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I. INTRODUCTION 



The problems o-f torpedo tracking have been studied, at 
the Naval Postgraduate School, by several researchers, who 
have implemented simulations that would allow analysis o-f 
tracking algorithm performance using the Extended Kalman 
Filter CIO. However, those studies have been limited to 
simulations done on large main-frame computers, without 
trying to adapt them to the real time processing system at 
the torpedo range. 

The objective here is to develop a version o-f the 
Extended Kalman Filter, adapted to the constraints o-f 
real-time processing, by generating a program in the C 
language. Also it is designed to run in the mi crocomputer 
C.I.E 680/30, currently utilized at the Naval Torpedo 
Station, Washington. 

The presentation herein tries to -follow as much as 
possible the chronological order , describing the problems in 
the order o-f appearance and the way they were solved. 
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II. PROBLEM DEFINITION 



A. RANSE TRACKING GEOMETRY 

Sets of hydrophone arrays are distributed in line along 
the extension of the test range. Each individual array 
consists of four independent elements, placed at a distance 
"d" apart, on the vertices of a cube, thus defining an 
orthogonal coordinate system in which the measurement of the 
transit times are made. Figure 1, shows the geometry. 

Four transit times, T c , T^ , T^, , corresponding to the 

four hydrophones in the array are measured and the 
positional information is computed from the measured transit 
times. The torpedos are equipped with sonar transmitters 
which are synchronized with a master clock, allowing new 
data every 1.31 seconds, with a range accuracy between 3 to 
30 feet. 

When multiple arrays are used, the signal from the 
closest hydrophone is defined as the basis for the time 
measurements and for the range calculations. If the computer 
time were not critical, an 18 hydrophone signal should be 
processed. 
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Figure 1: Geometry of a tracking array. 
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B. GENERAL EXTENDED KALMAN FILTER EQUATIONS 



A summary o-f the -filter and associated equations is 
given in this section, -following the order and the 
conventions in Cl 3. 

1 . Observation Equations 

For the three-dimensional problem, in which the 

z-position is considered invariant, a state space consisting 

o-f the three positions, X, Y and Z, and the two velocities, 

V and V de-fine the torpedo motion. Thus, the discrete 

x y 

linear estimation and the nonlinear observation equations 
are given by 

X (k+1 ) = 5 . X(k> + r • W (k) (2.1) 

Z (k) = h(X(k),k> + V(k> (2.2) 

where: £ and p are constant matrices; 

h is a nonlinear -function o-f the state X; 

W(k) is the plant excitation noise; 

V(k) is the measurement noise. 

The plant noise and the measurement noise are 
assumed Gaussian distributed with zero mean, and 
uncorrel ated . 

The last assumption implies that 



ECW(k) .W T ( j) 3 = Q'(k) s kj 
ECV(k) . V T ( j) 3 = R (k) s kj 
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where: &=1, -for k = j ; 

& = 0, -for k^j; 

2. Linearized Equations 

Linearization o-f the observation equation is 
obtained by using the Taylor Series to expand about the 
prediction o-f the trajectory -for the interval. 



Z(k) = H ( k ) . X (k) + V (k ) 
where. H(k) =■ | ,, (k > . £ (k/k _ u 

A 

X (k/k-1) is the estimated value (predicted) 
k, given the measurements until time k-1. 

3. State Error Vector 



(2.3a) 

(2.3b) 

at time 



X (k) = X (k) - X (k) 

4. Predicted State Error Vector 



X (k/k-1 ) = X (k/k-1 ) - X (k) 

5. Covariance o-f State Error 

P (k ) = ECX (k/k) .X T (k/k) 3 

6. Predicted Covariance 



P (k/k-1 ) = ECX <k/k-l> .X T (k/k-l> 3 

7. State Excitation Matrix 



Q ( k ) = p (k) .ECW(k) .W T (k) 3. p T (k) 

8. Measurement Noise Covariance Matrix 

R ( k ) = ECV(k) .V T (k) 3 
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9. Kalman Filter Equations 



P(k+l/k)=$.P(k/k> .£ T + Q(k> (2.4a) 

G(k)=P (k/k-1 >H T (k) CH (k > P( k/k-1 > H T (k > +R(k ) l” 1 (2. 4b) 

P(k>=CI-G(k> .H(k) l.P (k/k-1) (2. 4c) 

X (k+l/k>=£. X(k/k) (2. 4d ) 

Z (k/k-1 >=h(X (k/k-1 > , k) (2.4e> 

X (k/k)=X (k/k-1 )+G(k) CZ (k) -Z( k/k-1 ) 1 (2.4f > 



C. APPLIED EQUATIONS CIO 

In order to apply the Extended Kalman -filter to the 

torpedo tracking problem it is necessary to define the state 
vector and the matrices H, 3>, p , W, Q, and R. 

1 . Transit Times 

T (k)=r^rC (x+d/2) 2 + (y+d/2) a -t-(z+d/2) 3 3 1/a +v(k) (2.5) 

c VEL 

T <k)=r“rC (x-d/2) a + (y+d/2) a + <z+d/2> 2 ] i/a +v(k) (2.6) 

x VEL 7 

T y (k)= VEL C (x+d/2) a + (y-d/2) a + (z+d/2) a l 1/a +v(k) (2.7) 

( k ) = yg]“^ (x+d/2) 2 + (y+d/2 ) 2 + (z-d/2) 2 ] 1/a +v(k) (2.8) 

where: d=30 feet, is the distance between two hydrophones 

in a given array; 

VEL=4860 ft /sec, is the average sound speed at Dabob 
Bay. 

These equations must be linearized to apply Kalman 
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Filter Theory. The linearization is done, in this case, 
about the predicted trajectory, which is being generated by 
the -filter algorithm. 

The values o-f transit times are corrupted by noise, 
v ( k ) , which is assumed Saussi an with zero mean. The 
positions x, y, and z are all -functions o-f the time 
interval . 

2. State Equations 

The plant state equations are de-fined by 



■ X (k+1) ■ 




r X (k) +TV <k)+g 4 1 

X 1 


V (k+1) 

X 




V (k) +g_ 

X 2 


y (k+1) 


S 


y (k) +TV (k)+g, 
7 y 3 


V (k+1) 
Y 




V (k) +g 
y 4 


. z (k+1) . 




» z (k) +g s 



(2.9) 



where: x (k) , y(k) and z(k) are the position coordinates at 
time t <k ) ; 

(k) and V^(k) are the x and y components o-f the 
veloci ty ; 

the depth z (k) is maintained constant and any 
velocity in z is assumed random, Gaussian, 
uncorrelated and with zero mean; 

T is the sampling interval T=1.31sec; 

g A , g 2 , g 3 and g^ are excitation terms, included to 

take into account random variations in speed (y\. ), 

t 
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heading (y ) , and depth <y ) , which are assumed to 
9 t Z 

be independent, zero mean, rates o-f change. 

Typical torpedo maneuvering parameters are 



Oj. = 22 /sec; 
®t 



Etr v 2 i 
V t t 



9 f . = 36 -ft/sec 2 ; 



s 2 = ECy 2 1 
9 t 9 t 



a. = 1 -ft/sec; 



In state -form : 
X (k+1 ) = X (k) 
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ECyJ □ 



p. W(k) 



< 2 . 10 ) 



$ = 



1 T 0 0 0 
0 10 0 0 
0 0 1 T 0 
0 0 0 1 0 
0 0 0 0 1 



T 2 /2 



r = 



0 

0 



0 

0 T 

L 0 0 



T 2 /2 



0 

0 

0 

0 

T 



( 2 . 11 ) 



9 . 2 = (V /V. ) 9 f . 2 + V 2 J. 2 

x x t v t y 9 



<2. 12a) 
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(2. 12b) 



where: 



V- = ( VV‘< + V< 



ff. m m • = V V C ttfl /V. — 3 

x y x y v t t 9 



(2. 12c) 



Q is a symmetric matrix given by 





' qO 


qi 


q2 


q3 


q4 








q5 


q6 


q7 


q8 


q9 






Q = 


q 10 


qll 


ql2 


ql3 


ql4 




(2. 13) 




q 15 


ql6 


ql7 


ql8 


ql9 








. q20 


q2i 


q22 


q23 


q24. 






qO=(T 2 /2) 


. 2 . 
ff x* » 


qi 


= (T 3 


/2) a . 

> 


2 . 

i " f 


q2= <T 2 /2) 2 a ; 

M x y 



q3=(T 3 /2) 

* y 



q7=<T 3 /2) a ; 

x y 9 



q 12= <T 2 /2) 2 5 .? ; 



q4=0; 

q8=T 2 ; 



ql3=(T 3 /2) ; 



q6=T 2 ; 



q9=0; 

ql4=0; 



ql8=T 2 „., 2 ; 



q 19=0; 



q24=T 2 ^ 2 ; 



Z (k) = CT (k) T <k) T < k ) T <k) 3 
— c x y z 

dn. 0 0 0 



R ( k ) = 



2 

x 



a j~ 0 0 



zj 



16 



sec 



where: 



and 



Typically * T 2 =ff 2 T =« 2 T =«j = 10 
c x y z 



-1 o 





" hO 


hi 


h2 


h3 


h4 " 




h5 


h6 


h7 


h8 


h9 


H (k ) = 


hlO 


hi 1 


hl2 


h 13 


hl4 




hl5 


h 16 


hl7 


h 18 


hl9 




. h20 


h21 


h22 


h23 


h24. 


h0= <x ' (k) +d/2) /DENI ; 


hi 


=0; 





h3=0; 

h5=(x ' <k) -d/2) /DEN2; 
h7= (y ' <k)+d/2) /DEN2; 
h9=(z ' <k)+d/2) /DEN2; 



h2=(y ' (k)+d/2) /DENI; 
h4= (z ' ( k) +d/2) /DENI ; 
h6=0; 
h8=0; 

hlO=(x ' <k)+d/2) /DEN3; hll=0; 
hl2=<y ' <k)-d/2) /DEN3; hl3=0; 
h 1 4= ( z ' (k) +d/2) /DEN3; hl5=<x ' <k)+d/2) /DEN4; 
h 16=0; hl7=(y ' (k)+d/2)/DEN4; 

h 1 8=0 ; hl9=(z ' (k) -d/2) /DEN4; 

DEN1=C (x ' (k) +d/2) 2 + (y ' (k) +d/2) 2 + (z ' (k)+d/2) 2 I 1 /a 

DEN2=C (x ' (k ) -d/2) 2 + (y ' (k)+d/2) 2 + (z ' (k) +d/2) 2 l 1 /a 

DEN3=C (x ' (k)+d/2) a + (y' (k) -d/2) 2 + (z ' <k) +d/2) 2 3 1 /a 

DEN4=C (x ' (k) +d/2) 2 + (y ' (k)+d/2) a + (z ' (k) -d/2) 2 l 1 /a 
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D. THE SEQUENTIAL EXTENDED KALMAN FILTER APPROACH Cl] 



This approach assumes that all -four transit times 
measurements are statistically independent of each other. In 
this way one can process the transit times sequentially and 
independently. The result o-f each computation step is used 
immediately, as soon as it is obtained. Thus more accurate 
estimates o-f the -filter states are obtained. 

This procedure also circumvents the matrix inversion 
when determining the -filter gain, and allows one to 
disregard any particular time measurement that was found 
invalid, and not the total four times for a time slot 
(T=l . 31 sec ) . 

Erroneous transit times are detected by implementing a 
three sigma gate using the covariance of the measurement 
noise (R) and the covariance of the estimation error 
(P(k/k>. For i=l to 4 , 

gate(i) = 3* ( y<P . .max i mum) /4860 + R. . ) . 

J J n 

It can also be used to minimize position errors during 
high dynamic maneuvers, by repeating the calculations for 
the same time interval until the transit time residual error 
falls below a preset value. 
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III. THE IMPLEMENTATION PROBLEMS 



A. THE HARDWARE AND THE SOFTWARE PROBLEMS 

Since the objective is to use the developed so-ftware -for 
torpedo tracking at the NAVTORPSTA, in Keyport, Washington, 
there was no other choice but to implement the program to 
run on the C. I.E. 680/30 mi crocomputer , and to implement it 
in the C language. 

Two problems immediately arose. 

First, there was only badly written and incomplete 
documentation. The manual was found difficult to read and to 
understand. In addition, there was no information about the 
computer hardware. 

Secondly, poor software was available. The CIE 680/30 
is a Unix based machine, designed for business appl i cati ons , 
working with the Regulus operating system. As far as known, 
there is no software available (specifically written for the 
CIE) on the market. 

As a result of these problems, there was no full-screen 
editor available for the C.I.E machine, no communications 
program, and no built in graphics capability (although the 
first screen that appears after booting the system shows the 
message "CIE GRAPHICS SYSTEM #7"). 
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B. THE FAST APPROACH 



In order to be able to write a reasonably long and 
complex program, in a reasonable time, debug it and make it 
work, it would be necessary to have a good compiler, a good 
full screen editor and good documentation on software and 
hardware. 

It was then decided to make use of the known software 
and hardware information available for the IBIi-PC 
mi crocomputer . It was not difficult to find Public Domain 
Programs able to communicate with Unix based machines, to 
emulate the VT-100 terminals, and to transfer files. Also it 
was not difficult to find a powerful easy to use full-screen 
editor for the IBM— PC. 

The program was then written, trying to keep the C code 
the most standard possible, using the Lattice C compiler. 

It was executed first in the IBM-Pc, then transferred 
(source code) to the C.I.E using a public domain program 
(SIMTERM) . It was then adapted, making minor changes, and 
recompiled on the C.I.E to generate the executable code. 

C. ODDS AND EVENS OF C 

The C language is not very high level but it is a very 
powerful language, allowing bit operations, low level read 
and write, system calls and easy interfaces with machine 
language. The general mathematical expressions can be kept 
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the same way as in Fortran, but some new operations, 
-functions and expressions are also used. 

Incrementing a variable: 

"i=i + lj" can be ,, i++" or "++i", depending if you 
first use the variable (++i> and increment it or 
vice versa (i++>. 

Incrementing i by j: "i+“j;" 

Do loop, testing the counting variable after using it: 
"for <i=0; iCimax ; i++) ; " 

Blocks are delimited by "< 

Every program, subprogram, functions or procedures are 
seen as FUNCTIONS in C, including the main program. This is 
indicated by the calling statements as in: 

mainO .... for the main program that is not 

passing any arguments; 

plot(x,y) .... for a routine that passes X and 

Y vectors for plotting, and so on; 

C uses passing by value approach. So the value of the 
variable "x" is copied in and used inside a function. Arrays 
are equivalent to pointers and thus they can return new 
values from functions. 

There are automatic, local, global and external 
variables. There are character, integer, long integer, 
string, floating point (double and single precision), 
string, and enumeration data types. Short integers have 
standard size. Double precision floating point allows one to 
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in the IBM-PC 



represent numbers in the range 



io± 30 ’ 



(Lattice C compiler) and in the range 10 ±38 in the CIE. 
There are IF , IF THEN ELSE, WHILE, DO .. WHILE, and CASE 
statements. 

There are no input and output statements as in Fortran 
or other languages. Input-output is done by standard 
■functions, which must be care-fully used. 

Header -files and " ttinclude 11 statements are used to 
hide -from the user de-finitions of parameters that are 
machine dependent or contain environment-specific 
information. "#define" statements are used to define 
constants and macros and to hide machine dependent code 
segments. 

Reference C41 is the definition manual of the language. 
The other references for the C language were used primarily 
to look over the example programs, and functions. 
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IV. THE REAL-TIME KALMAN FILTER PROGRAM IN C 



A. THE INITIAL APPROACH 

The first idea was the simple translation of the program 
described in reference C 1 □ to the C language, without 
worrying about specific problems that would arise when 
trying, later on, in a more realistic approach, to simulate 
as close as possible the actual running conditions. 

The initial program had a structure similar to the 
Fortran program in C 1 □ , that is, a main program, the actual 
Kalman Filter, and several functions to calculate the 
trajectory, to add, transpose and multiply matrices and 
to multiply two vectors. All the floating point operations 
were done in double precision and the matrices were defined 
as two dimensional arrays. 

The initial approach failed in many aspects. 

It did not have enough speed, producing, on the average, 
one estimate every 22 sec, in an IBM-PC without math- 
coprocessor (8087), running at 5 MHz. The same program 
generated a new estimate every 5 sec in an IBM-PC with an 
8087 chip. It did not have the main frame graphics 
capability, readily available and, finally, due to the way 
the C language passes parameters to the subroutines, the 
matrix routines worked correctly only if matrices were 
defined as square matrices. 
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B. OPTIMIZATION OF EXECUTION TIME 



Although the C.I.E microcomputer runs a Motorola 68000 
cpu at 8 MHz, and the IBM-PC runs at 5 MHz, executing the 
same program in both machines showed a difference of 10% to 
20% in execution time. In this way, optimizing the execution 
in the IBM-PC (with 8087) would be a valid approach since it 
would allow a safety margin, and would require almost the 
same effort. 

Optimization of execution time led to : 

1. Transforming all the matrices from bi di mensi onal to 
uni di mensi onal arrays (or vectors). That saved memory space, 
and forced the access to the elements to conform with the 
way these are stored (C stores array elements by rows), thus 
speeding up retrieving when needed. 

2. Eliminating functions, actually by having everything 
in the main program. 

3. Eliminating all the unnecessary operations. That 
involved restructuri ng the program to avoid loops when 
possible and reasonable, avoiding multiplying by zero, 
avoiding adding zeros, and avoiding repeating calculations 
when the result was known to be constant. 

4. Avoiding double precision where possible, and when 
improvement of the execution time could be achieved without 
compromising the results. The Lattice C compiler, in the 
IBM-PC, is defined for precision (all the floating point 
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calculations are done in double precision). Then, if single 
precision is desired, you lose in both, precision and speed. 
The CIE machine calculations uses the Motorola "Fast 
Floating Point", a format which is not compatible with the 
IEEE format. The IEEE floating point values lose some 
precision due to the fact that the calculations use the fast 
floating point routines and a translation to and from the 
FFP format C101. So, the FFP format was used, in the CIE. 

5. Writing single precision routines to avoid calling 
standard mathematical routines, which are all double 
precision, in C. 

6. Reproducing more closely the way the final program 
would have to work, when in the real time application. 

7. Testing the program for several values and 
parameters, to determine bounds that would allow results of 
good precision without increasing the execution time. It was 
found, for example, that the results are highly sensitive to 
the way the columns of the matrix H are calculated, thus, 
requiring calculation of H using double precision. 

8. Measuring execution time of tasks inside the 
program. 

9. Developing software tools to allow for interactive 
execution and checking of correctness of the results. 
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V. TESTING 



A. CHECKING THE TRAJECTORY-FOLLOWING PERFORMANCE 

The trajectory -following performance was veri-fied -for 
the following trajectories : 

1. Straight line parallel to the array axis, and 
y-coordinate = 0.0 , single array tracking; 

2. Straight line parallel to the array axis and 
y-coordinate = 2121.0 , single array tracking; 

3. Straight line, 45- inclination relatively to the 
array axis, single array; 

4. Straight line parallel to the array axis, long run, 
multiple array tracking; 

5. Straight line and circular maneuver, short run, slow 

turn (lO.Odg/sec , 1/4-G), single array tracking; 

6. Straight line and circular maneuver, long run, slow 

turn (lO.Odg/sec , 1/4-G), multiple array tracking; 

7. Straight line and circular maneuver, short run, fast 

turn <20dg/sec , 1-G) , single array tracking, adaptive 

maneuver implemented with 4 iterations allowed; 

9. Straight line and circular maneuver, long run, fast 
turn (20 dg/sec , 1-G), multiple array tracking, adaptive 

maneuver implemented with 4 iterations allowed; 
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TABLE 1 , and TABLE 2 summarize the results in terms o-f 
maximum absolute error -for the IBM and the CIE microcom- 
puters, respectively. For the single array cases, only the 
region inside a 3000 -feet radius around the center o-f the 
array being used is considered in determining the errors. 

The correspondi ng plots can be -found in the -following 

— 1 0 

pages. The plots were obtained with the value 10 for the 
elements in the diagonal of the matrix R (the Error 
Covariance Matrix) for the IBM-PC microcomputer and with the 
— e 

value 10 for the CIE. The plots are numbered sequentially 
and contain headings to identify and relate them to the 
Table 1 and Table 2. The actual time is obtained by 
multiplying the time scale by the length of the time slot, 
1.31 seconds. 

B. CHECKING THE EXECUTION TIME REQUIREMENT 

The execution time in the IBM-PC, was verified by 
checking the printouts containing the values of the internal 
computer time for a cycle. Later, an "waiting loop" was 
introduced to generate an interval of 1.31 sec between each 
data input (that is, reading files TRJ.DAT and ZIC.DAT). 

Since the "reading internal time" was not incorporated 
in the CIE program version (CIEKR1), and since that proved 
to be a faster machine, the time requirement was just 
checked against a chronometer for 150 iterations. 
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TABLE 1 



IBM-PC MICROCOMPUTER 



RUN 


MAX. ABS. POS. 


ERROR 


TRAJEC 


ARRAY 


INITIAL 


# 


X_ERR 


Y_ERR 


Z_ERR 


TYPE 


TYPE 


CONDIT. 


1 


0. 025 


4.5 


2 


1 i near 


singl 


x =3000 
y = 0 
Vx =-50 
Vy= 0 


2 


4.5 


5. 4 


2.5 


1 i near 


singl 


x =3000 
y =2121 
Vx=— 33. 8 
Vy= 0 


3 


6.0 


6. 0 


2.7 


1 inear 


singl 


x =3000 
y =-3000 
Vx=-33. 8 
Vy= 33.8 


4 


0.2 


5. 3 


1.8 


1 i near 


mul tp 


x =34000 
y =6050. 
Vx=-84. 4 
Vy= 0 


5 


1.0 


4.3 


5.6 


circl 
lOdg/s 
t 0 =40 
t-f =70 


si ngl 


x =3000 

y = 0 

Vx =-50 
Vy= 5 


6 


17 


3.3 


2.8 


circl 
lOdg/s 
t 0 =40 
tf =70 


mul tp 


x =10000 
y = 0 
Vx =-50 
Vy= 5 


7 


2.4 


7 


3.7 


circl 
20dg/s 
t 0 =35 
t-f =45 


si ngl 


x =5000 
y = 0 
Vx =-92 . 5 
Vy= 0 


8 


22 


5.2 


12 


ci rcl 
20dg/s 
t 0 =120 
t-f = 130 


mul tp 


x =20000 
y = 0 
Vx =-92 . 5 
Vy= 5 
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TABLE 2 



CIE MICROCOMPUTER 



RUN 


MAX. ABS. POS. 


ERROR 


TRAJEC 


ARRAY 


INITIAL 


# 


X_ERR 


Y_ERR 


Z_ERR 


TYPE 


TYPE 


CONDIT. 


1 


0.031 


4 


1 


linear 


singl 


x =3000 
y = 0 
Vx =-50 
Vy= 0 


2 


7. 1 


10 


1.2 


linear 


singl 


x =3000 
y =2121 
Vx =-33 . 8 
Vy= 0 


3 


3 


3 


0.9 


1 i near 


singl 


x =3000 
. y =-3000 
Vx =-33 . 8 
Vy= 33.8 


4 


0.2 

r 


3. 5 


0. 7 


1 i near 


mul tp 


x =34000 
y =6050 
Vx=-B4. 4 
Vy= 0 


5 


— 


— 


— 


circl 
lOdg/s 
t 0 =40 
t-f =70 


singl 


x =3000 
y = 0 
Vx=-50 
Vy= 5 


6 


— 


‘ 


— 


circl 
lOdg/s 
t 0 =40 
t-f =70 


mul tp 


x =10000 

y = 0 

Vx =-50 
Vy= 5 


7 


— 


— 


— 


circl 
20dg/s 
t 0 =35 
t-f =45 


singl 


x =5000 

y = 0 

Vx =-92 . 5 
Vy= 0 


8 


— 


— 


— 


circl 
20dg/s 
t 0 =120 
tf =130 


mul tp 


x =20000 

y = 0 

Vx=— 92. 5 
Vy= 5 
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Figure 2. IBM-PC - True trajectory of the torpedo in the 
area of a single array. Initial conditions i x=3000 ft, y=0 
ft, Vx=»-50 ft/sec, Vy=0 ft/sec. 
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Figure 3. IBM-PC - 
straight run in the 
conditions: x=3000 -ft. 



Estimate of torpedo position during a 
area of a single array. Initial 
y=0 ft, Vx=-50 ft/sec, Vy=0 ft/sec. 
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Figure 4. IBM— PC — 
straight run in the 
conditiansi x=3000 ft. 



Error in torpedo position during a 
area of a single array. Initial 
y=0 ft, Vx=*-50 ft/sec, Vy=0 ft/sec. 
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Figure 5. IBM-PC - Error in torpedo position during a 
straight run in the area o-f a single array. Initial 
conditionsi x=3000 -ft, y=0 -ft, Vx=-50 -ft/sec, Vy=0 -ft/sec. 
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Figure 6 . IBM-PC - 
run in the area 
x=3000 -ft, y»0 ft, 



Error in torpedo depth during a straight 
of a single array. Initial conditions: 
Vx=-50 ft/sec, Vy=0 ft/sec. 
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Figure 7. IBM-PC - 
straight run in the 
conditions: x=3000 -ft, 



Variance o-f position 
area o-f a single 
y=0 -ft, Vx=-50 -ft/sec, 



error during a 
array. Initial 
Vy=0 -ft/sec. 
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Figure 8. IBM-PC - 
straight run in the 
conditions! x*3000 -ft, 



Variance of position error during a 
area of a single array. Initial 
y-0 ft, Vx*-50 ft/sec, Vy«0 ft/sec. 
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Figure 9. IBM-PC - 
straight run in the 
conditions: x=3000 -ft, 



Variance of depth 
area o-f a single 
y«*0 ft, Vx »-50 ft/sec 



error during a 
array. Initial 
Vy=0 ft/sec. 
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Figure 10. IBM-PC 
during a straight run 
conditions: x=3000 ft 



- Filter estimate of mean-square error 
in the area of a single array. Initial 
y=0 ft, Vx=-50 ft/sec, Vy=0 ft/sec. 
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Figure 11. IBM-PC - Filter estimate o-f mean-square error 
during a straight run in the area of a single array. Initial 
conditions! x»3000 ft, y=0 ft, Vx=-50 ft/sec, Vy=0 ft/sec. 
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Figure 12. IBM-PC - Filter estimate o-f mean-square error 
during a straight run in the area o-f a single array. Initial 
conditionsi x=3000 -ft, y-0 -ft, Vx=-50 -ft/sec, Vy=0 ft/sec. 
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Figure 13. IBM-PC - Real time plot of estimated trajectory 
of the torpedo in the area of a single array. Initial 
conditions. x=3000 ft, y = 2121 ft, Vx = —33.8 ft/s, Vy=0 ft/s. 
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Figure 14. IBM-PC - True 
area o-f a single array. 
y=2121 -ft, Vx=-33.B ft/sec, 



trajectory o-f the torpedo in the 
Initial conditions : x=3000 ft, 
Vy=0 ft/sec. 
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Figure 15. 
straight run 
ares x=3000 



IBM-PC - Estimate o-f torpedo 
in the area o-f single array, 
■ft, y=2121 -ft, Vx=— 33. 8 ft/s. 



position during a 
Initial conditions 
Vy=0 -ft/s. 
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Figure 16. IBM-PC 
straight run in the 
conditions] x=3000 -ft, 



Error 
area of 
y aa 2121 ft 



in torpedo position during a 
a single array. Initial 
, Vx=»— 33.8 ft/s, Vy=0 ft/s. 



44 



_ u~> 




Figure 17. IBM-PC - Error 
s ^ ra i9ht run in the area of 
conditions! x»3000 ft, y=2121 ft 



in torpedo position during a 
a single array. Initial 
i 33. 0 ft/s, Vy=0 ft/s. 
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Figure 18. IBM— PC — 
straight run in the 
conditions: x=3000 ft, 



Error in torpedo depth during a 
area of a single array. Initial 
y=2121 ft, Vx=-33.8 ft/s, Vy=0 ft/s. 
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Fi gure 19. IBM PC ~ Filter estimate of mean - square error 
during a straight run in the area o-f a single array. Initial 
conditions: x=3000 -ft, y=2121 ft, Vx=-33.B ft/s, Vy=0 ft/s. 
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Figure 20. IBM-PC - Filter estimate of mean-square error 
during a straight run in the area of a single array. Initial 
conditionst x=3000 ft, y=2121 ft, Vx=-33.8 ft/s, Vy=0 ft/s. 
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Figure 21. IBM-PC - Filter estimate of mean-square error 
during a straight run in the area of a single array. Initial 
conditionsi x=3000 ft, y=2121 ft, Vx=-33.8 ft/s, Vy=0 ft/s. 
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F i gure 
of the 
t i ons: 



22. IBM-PC - Real ti 
torpedo in the area of 
x=3000 ft, y=-3000 ft, 



me plot 
a singl 
Vx=— 33. 



of estimated trajectory 
e array. Initial condi- 
8 ft/s, Vy*=-33.8 ft/s. 



50 




Figure 23. 
area o-f a 
y=— 3000 -ft. 



IBM-PC - True 
single array. 
Vx=-33. 8 -ft/sec 



f 



trajectory o-f the torpedo in the 
Initial conditions : x=«3000 ft, 
Vy=33.B -ft/sec. 
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Figure 24. 
straight run 
are: x=3000 



IBM-PC - Estimate o-f torpedo 
in the area o-f single array, 
■ft, y=— 3000 -ft, Vx=-33. 8 -ft/s. 



position during a 
Initial conditions 
Vy=33.8 -ft/s. 
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Figure 
strai ght 
di tions: 



25. IBM-PC - Error in torpedo position during a 
run in the area of a single array. Initial con- 
x=3000 ft, y=— 3000 ft, Vx=-33. Q ft/s, Vy=33.B ft/s. 
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Figure 26. IBM-PC - Error 
straight run in the area of 
ditions: x=3000 ft, y=-3000 ft 



in torpedo position during a 
a single array. Initial con- 
, Vx=-33.S ft/s, Vy=»33. 8 ft/s. 
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Figure 27. 
straight run 
s x=3000 -ft. 



IBM-PC — Error in torpedo depth during a 
in the area o-F single array. Initial conditions 
y=— 3000 -ft, Vx=— 33. S -ft/s, Vy=*33. 8 -ft/s. 
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Figure 28. IBM-PC — Filter 
for a straight run in the area 
ditions: x=3000 ft, y=— 3000 ft 



estimate of mean-square error 
of single array. Initial con— 
, Vx=-33.8 ft/s, Vy=33. 8 ft/s. 
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Figure 29. IBM-PC - Filter 
for a straight run in the area 
ditionss x=3000 ft, y=*-30Q0 ft 



estimate of mean-square 
of single array. Initial 
, Vx=— 33.B ft/s, Vy=33. 8 



error 

con- 



f t/s. 
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Figure 30. IBM-PC - Filter estimate o-f mean-square error 
•for a straight run in the area of single array. Initial con- 
ditions: x=3000 -ft, y=*-3000 -ft, Vx=-33.B -ft/s, *^=*33.3 -ft/s. 
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Figure 31. IBM-PC - True trajectory o-f the torpedo in the 
during a straight runthrough multiple arrays. Initial condi- 
tions! k=<» 34000 -ft, y=6050 -ft, Vx=*-04.4 -ft/sec, Vy=0 -ft/sec. 
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Figure 32. IBM-PC - Estimate o-f torpedo position during a 
straight run through multiple arrays. Initial conditions 
are: x=34000 -ft, y=6050 -ft, Vx=-B4.4 -ft/s, Vy=0 it/s. 
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Figure 33. IBM-PC - 
straight run through 
arei ><=*34000 ft, y*»6050 



Error in torpedo position during a 
multiple arrays. Initial conditions 
ft, Vx«-84.4 ft/s, Vy=0 ft/s. 
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Figure 34. IBM-PC - Error in torpedo position during a 
straight run through multiple arrays. Initial conditions 
ares x=34000 ft, y=6050 ft, Vx=-B4.4 ft/s, Vy=0 ft/s. 
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Figure 35. IBM-PC - Error in torpedo depth during a 
straight run through multiple arrays. Initial conditions 
ares x=34000 -ft, y=6050 -ft, Vx=-B4.4 -ft/s, Vy=0 ft/s. 
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Fi gure 
for a 
tions: 



36. IBM— PC 

straight run 
x =34000 ft, y= 



Filter estimate of mean— square 
through multiple arrays. Initial 
6050 ft, Vx=-B4.4 ft/s, Vy=0 ft/s 



error 
condi - 
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Figure 37. IBM-PC - Filter estimate of mean-square error 
for a straight run through multiple arrays. Initial condi- 
tions: x =34000 ft, y=6050 ft, Vx=-B4.4 ft/s, Vy=0 ft/s. 
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Figure 3B. IBM-PC - Filter estimate of mean-square error 
for a straight run in the area of single array. Initial con- 
ditions: x=34000 ft, y=6050 ft, Vx=-B4.4 ft/s, Vy=0 ft/s. 
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Figure 
of the 
tionsi 



39. IBM-PC - 
torpedo in the 
x=3000 -ft, y=0 



Real time 
area o-f a 
ft, Vx=50 



plot of estimated trajectory 
single array. Initial condi— 
ft/s, Vy=5 ft/s. 
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Figure 40. IBM-PC 
10 dg/s maneuver 
conditions! x=3000 



- True trajectory of the torpedo during a 
in the area of a single array. Initial 
ft, y=»0 ft, Vx=-50 ft/sec, Vy=5 ft/sec. 
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Figure 41. IBIi-PC - 
10 dg/s maneuver in 
conditions: x=3000 ft, 



Estimate of torpedo position during a 
the area of a single array. Initial 
y=0 ft, Vx=-50 ft/sec, Vy=5 ft/sec. 
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Figure 42. IBM-PC - Error in torpedo positi 
dg/s maneuver in the area o-f a single 
conditions: x=3000 -ft, y=0 -ft, Vx=-50 -ft/sec, 



on during a 10 
array. Initial 
Vy=5 -ft/sec. 
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Figure 43. IBM-PC - Error 
dg/s maneuver in the area 
conditions: x=3000 ft, y=0 ft 



in torpedo positi 
of a single 
, Vx =-50 ft/sec, 



on during a 10 
array. Initial 
Vy=5 ft /sec. 
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Figure 44. IBM-PC - Error in torpedo depth during a 10 dg/s 
maneuver in the area o-f a single array. Initial conditions: 
x=3000 ft, y=0 ft, Vx =-50 ft/sec, Vy=5 ft/sec. 
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Figure 45. IBM-PC - True trajectory of the torpedo during 
a 10 dg/s maneuver through multiple arrays. Initial 
conditionsi x=1000 ft, y=0 ft, Vx=-50 ft/sec, Vy=5 ft/sec. 
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Figure 46. 
10 dg/s 
conditions: 



I BM— PC - 
maneuver 
x=1000 -ft, 



Estimate of torpedo position during a 
thruough multiple arrays. Initial 
y=0 ft, Vx=— 50 ft/sec, Vy=5 ft/sec. 
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Figure 47. 
dg/s maneuver 
x = 1000 -ft, y=0 



IBM— PC — Error in torpedo position 
through multiple arrays. Initial 
‘f'tj Vx =-50 ft/sec f Vy=5 ■ft/sec. 



during a 10 
conditions: 
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Figure 48. IBM-PC - Error in torpedo position during a 10 
dg/s maneuver through multiple arrays. Initial conditionss 
k = 1000 -ft, y=0 -ft, Vx =-50 -ft/sec, Vy=5 ft/sec. 
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Figure 49. IBM-PC - Error in torpedo position during a 10 
dg/s maneuver through multiple arrays. Initial conditions: 
x = 1000 -ft, y=0 -ft, Vx =-50 -ft/sec, Vy=5 -ft/sec. 
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Figure 50. IBM-PC - True trajectory o-f the torpedo during a 
20 dg/s maneuver in the area o-f a single array. Initial con- 
ditions: x=5000 -ft, y=0 -ft, Vx=-92.5 -ft/sec, Vy=0 ft/sec. 
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Figure 51. IBM-PC - 
20 dg/s maneuver in 
conditions: x=5000 -ft, 



Estimate of torpedo position during a 
the area of a single array. Initial 
y=0 ft, Vx=-92. 5 ft/sec, Vy=0 ft/sec. 
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Figure 52. IBM-PC - Error in torpedo position duri 
dg/s maneuver in the area of a single array, 
conditions: x=5000 ft, y=0 ft, Vx=-92.5 ft/sec, Vy=0 



ng a 20 
Ini ti al 
f t/sec . 
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Figure 53. IBM-PC - Error in torpedo position during a 20 
dg/s maneuver in the area of a single array. Initial 
conditions: x=5000 ft, y=0 ft, Vx=-92.5 ft/sec, Vy=0 ft/sec. 
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Figure 54. IBM-PC - Error in torpedo depth during a 20 dg/s 
maneuver in the area o-f a single array. Initial conditions: 
x=5000 ft, y=0 -ft, Vx=-92.5 -ft/sec, Vy=0 ft/sec. 
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time plot o-f estimated trajectory 
multiple arrays. Initial condi- 
Vx=-92.5 -ft/sec, Vy=0 -ft/sec. 



Figure 55. IBM-PC - Real 
o-f the torpedo through 
tions: x=20000 -ft, y=0 -ft, 
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Figure 56, IBIi-PC - True trajectory o-f the torpedo in the 
during a 20 dg/s maneuver through multiple arrays. Initial 
conditions: x=20000 -ft, y=0 -ft, Vx=-92.5 -ft/s, Vy=0 -ft/s. 
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Figure 57. 
20 dg/s 
conditions: 



IBM-PC - Estimate 
maneuver through 
x =20000 ft, y=0 ft, 



of torpedo position 
multiple arrays. 
Vx=-92.5 ft/s, Vy=0 



during a 
Ini tial 
ft/s. 
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Figure 58. IBM-PC - Error in torpedo position during a 20 
dg/s maneuver through multiple arrays. Initial conditions: 
x =20000 -ft, y=0 ft, Vx=— 92. 5 ft/s, Vy=0 ft/s. 
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Figure 59. IBM-PC - Error in torpedo position during a 20 
dg/s maneuver through multiple arrays. Initial conditions: 
x =20000 -ft, y=0 -ft, Vx =-92. 5 tt/s, Vy=0 tt/s. 
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Figure 60. IBM-PC - Error in torpedo position during a 20 
dg/s maneuver through multiple arrays. Initial conditions: 
h =20000 -ft, y=0 -ft, Vx=— 92. 5 ft/s , Vy=0 ft/s. 
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Figure 61. CIE - True trajectory o-f the torpedo in the 
area of a single array. Initial conditions : x=3000 ft, y=0 
ft, Vx=*-50 ft/sec, Vy=0 ft/sec. 
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Figure 62. CIE - Estimate of 
straight run in the area of a 
conditions: x=3000 ft, y=0 ft, Vx=- 



torpedo position 
single array. 
50 ft/sec, Vy=0 



during a 
Initial 
f t/sec . 
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Figure 63. CIE - Error in torpedo position 
straight run in the area of a single array, 
conditions: x=3000 ft, y=0 ft, Vx=-50 ft/sec, Vy=0 



during a 
Initial 
ft/sec. 
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Figure 64. CIE 
straight run in 
conditions: x=3000 



- Error in 
the area o-f 
-ft, y=0 -ft, Vx 



torpedo position 
a single array 
=-50 -ft /sec, Vy=0 



during a 
Initial 
■ft/sec . 
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Figure 65. CIE — Error 
run in the area of a 
x *3000 ft, y=0 ft, Vx=-50 



in torpedo depth during 
single array. Initial 
ft/sec, Vy=0 ft/sec. 



a straight 
conditions: 
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Figure 66. 
straight run 
conditions! x- 



CIE — Variance of position error during a 
in the area of a single array. Initial 
=3000 ft, y=0 ft, Vx=— 50 ft/sec, Vy=0 ft/sec. 
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Figure 67 . CIE - Variance o-f position 
straight run in the area o-f a single 
conditions: x=3000 -ft, y=0 -ft, Vx=-50 -ft/sec 



error during a 
array. Initial 
Vy=0 -ft/sec. 
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Figure 68. CIE — 
run in the area 
x=3000 ft, y=0 ft, 



Variance of depth error during a straight 
of a single array. Initial conditions: 
Vx=-50 ft/sec, Vy=0 ft/sec. 
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Figure 69. CIE — 
during a straight run 
conditions: x=3000 -ft 



Filter 
in the 
, y=0 ft 



estimate oi mean-square error 
area of a single array. Initial 
, Vx=*-50 ft/sec, Vy=0 ft/sec. 
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Figure 70. CIE - 
during a straight run 
conditions: x=*3000 -ft 



Filter 
in the 
, y=0 -ft 



estimate o-f mean-square error 
area of a single array. Initial 
, Vx=-50 ft/sec, Vy=0 ft/sec. 
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Figure 71. CIE - 
during a straight run 
conditions: x=3000 ft 



Filter 
in the 
, y=0 ft 



estimate of mean-square error 
area of a single array. Initial 
, Vx=-50 ft/sec, Vy=0 ft/sec. 
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Figure 72. CIE - True 

area of a single array. 
y=2121 ft, Vx=— 33.8 ft/sec. 



trajectory of the torpedo in the 
Initial conditions : x=3000 ft, 
Vy=0 ft/sec. 
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Figure 73. CIE - Estimate o-f torpedo 
straight run in the area o-f single array, 
ares x = 3000 ft* y = 2121 ft* Vx=— 33.8 -ft/s. 



position during a 
Initial conditions 
Vy=0 -ft/s. 
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Figure 74. CIE - Error in 
straight run in the area of 
conditions! x=*3000 ft, y=2121 ft 



torpedo position during a 
a single array. Initial 
Vx=-33.B ft/s, Vy=0 ft/s. 
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Figure 75. CIE 
straight run in 
conditions! x**3000 



- Error in 
the area of 
ft, y=2121 ft 



torpedo position during a 
a single array. Initial 
, Vx=-33.8 ft/s, Vy=0 ft/s. 
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Figure 76. CIE - Error 
run in the area of a 
x=»3000 ft, y=2121 ft, Vx- 



in torpedo depth during a straight 
single array. Initial conditions: 
33.8 ft/s, Vy»0 ft/s. 
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Figure 77. CIE - Filter estimate of mean-square error 
during a straight run in the area of a single array. Initial 
conditions: x=3000 ft, y=2121 ft, Vx=— 33.8 ft/s, Vy=0 ft/s. 
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Figure 78. CIE - Filter estimate of mean-square error 
during a straight run in the area of a single array. Initial 
conditions: x=3000 ft, y=2121 ft, Vx=-33.B ft/s, Vy=0 ft/s. 
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Figure 79, CIE - Filter estimate of mean-square error 
during a straight run in the area of a single array. Initial 
conditions: x=3000 ft, y=2121 ft, Vx=-33.B ft/s, Vy=0 ft/s. 
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Figure 80. 
area o-f a 
y— 3000 -ft, 



CIE 
si ngl e 
Vx=— 33. 8 



— True 
array . 
■f t/sec 



trajectory o-f the torpedo in 
Initial conditions : x=3000 
, Vy=33. 8 i t/sec. 



the 
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Figure 81. CIE — Estimate of 
straight run in the area o-f single 
are: x=3000 -ft, y=-3000 -ft, Vx=-33 



torpedo position during a 
array. Initial conditions 
8 f t/s i Vy=33. 8 ft/s. 
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Figure 82. CIE 
straight run in 
tions: x=3000 -ft, 



- Error in torpedo position during a 
the area of a single array. Initial condi- 
y=-3000 ft, Vx=— 33.B ft/s, Vy=33.S ft/s. 
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Figure 83. CIE 
straight run in 
tionss x=3000 -ft, 



- Error in torpedo position during a 
the area of a single array. Initial condi- 
y=— 3000 -ft, Vx=-33.B ft/s, Vy=33.8 ft/s. 
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Figure 84. CIE - Error in torpedo depth during a straight 
run in the area of single array. Initial conditions* x=3000 
ft, y=— 3000 ft, Vx—33.8 ft/s, Vy=33.8 ft/s. 
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Figure 85. CIE - True trajectory of the torpedo in the 
during a straight runthrough multiple arrays. Initial condi- 
tions: x =34000 -ft, y=6050 -ft, Vx=-B4.4 -ft/sec, Vy=0 -ft/sec. 
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Figure 86. CIE - Estimate of torpedo 
straight run through multiple arrays, 
arei *=34000 ft, y=6050 ft, Vx=-B4.4 ft/s, 



position during a 
Initial conditions 
Vy=0 ft/s. 
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Figure 87. CIE — Error in torpedo position during a 
straight run through multiple arrays. Initial conditions 
are i x=34000 -ft, y=6050 ft, Vx=-84.4 ft/s, Vy=0 ft/s. 
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Figure 88. CIE - Error in 
straight run through multiple 
are: x =34000 ft, y=6050 ft, Vx=- 



torpedo 
arrays. 
84.4 ft/s 



position during a 
Initial conditions 
Vy=0 ft/s. 
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Figure 89. CIE - Error in torpedo depth during a straight 
run through multiple arrays. Initial conditions are: x=34000 
ft, y=6050 ft, Vx=-84. 4 ft/s, Vy=0 ft/s. 
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VI. COMMENTS AND CONCLUSIONS 



There are slightly different versions of the program, 
for the IBM-PC and for the CIE mi crocomputer . However, the 
Sequencial Extended Kalman Filter program presented have 
many common characteri sties. The execution time per cycle, 
in both machines, is less than the sample interval for the 
data. The program is written in the "C" programming 
language, and it is meant to be reasonably portable. Double 
precision calculations are widely used, and allowed in many 
cases to bound errors without repeating the calculations for 
the same time interval. 

Calculations are performed using an adaptive Q matrix. 
That is, the matrix Q is recalculated each iteration, 
instead of using a constant value. The trajectory simulation 
routine was implemented using geometrical approach. The 
tangential speed is assumed constant during the whole run. 

The Kalman Filter can track through single and multiple 
arrays. The algorithm always uses the coordinates of the 
closest array in the calculations, for the multiple array 
case. There is essentially, in either CIEKR1 and KR, only a 
main program without subroutine calls. The bi di mensi onal 
matrices were transformed and are used as vectors. Most 
loops were eliminated to keep the execution time within the 
required limits. 
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The CIE version can also run in the IBM-PC but the IBM- 
PC version runs only in the IBM since it calls assembly 
language routines to generate graphic output. The IBM 
version is slower than the CIE version and the IBM-PC 
microcomputer must run with its 8087 math coprocessor chip 
to satisfy the speed requirements. Both machines were 
configured with hard disks, and the data files were read 
from and written to the hard disks. Both, the IBM-PC and the 
CIE versions, will run on the IBM-AT. 

There is almost no documentation or software available 
for the CIE mi crocomputer . The lack of documentation, the 
lack of a good compiler, a full-screen editor, and the lack 
of graphics capability were major problems in using the CIE. 

The improvement in the execution time was more 
noticeable in the CIE than in the IBM. This is probably due 
to the presence of the math coprocessor chip, 8087, and (or) 
due to a better compiler, in the IBM-PC. The use of the "— f" 
("fast floating point format") option, when compiling code 
on the CIE, seemed to help both, the speed of execution and 
the precision of the results. 

The programs were tested for torpedo speeds from 20 
knots (33.8 ft/s) to 55 knots (92.8 ft/s) and turn rates of 
up to 20 dg/sec, or maneuvers of up to 1-G. The magnitude of 
the position errors for linear trajectories were maintained 
less than 10 ft in either computer. The CIE could not track 
any of the circular maneuvers (run numbers 6, 7 and 8 in 
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Table 2). In the IBM-PC, keeping the maneuver within the 
tracking radius o-f a single array ( + 3000 -ft around the 
array center) , the magnitude o-f position errors were 
maintained less or equal to 7 feet for an 1-G maneuver and 
less or equal to 22 feet for the multiple array case. 

The high error obtained for the circular maneuvers in 
either the single or multiple array case reflects the 
algorithm inability to follow fast turn circular 
tra jector i es. In C13 such high errors were reduced to values 
comparable to the errors found in the straight runs, by 
adding the adaptive Q matrix to FKK to produce PKKM1 and 
repeating the calculations for the same time interval. This 
was called "adaptive maneuver". The "adaptive maneuver" as 
implemented (see segment 2, in the algorithm, in Appendix A) 

— 1 0 

was found effective for the IBM-PC when the value 10 was 
used for the diagonal elements of the matrix R. However, to 
keep the execution time per cycle within 1.31 sec, the 
number of adaptive maneuvers performed had to be limited. 
Almost no improvement in reducing tracking errors was 

— 8 

achieved when the value used was 10 . For the later case a 
great deal of degradation in the execution time was observed 
and the time interval was exceeded even when just one more 
iteration (optimum maneuver) was executed. The procedure did 
not work on the CIE, which could not handle numbers less 

than 10 reliabily. The "adaptive maneuver" feature was 
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utilized only for the runs 7 and 8 (only 4 iterations were 
allowed) and have produced, for the 1-G maneuvers, results 
which are comparable to the results for the 1/4-G maneuvers. 
The single array cases showed smaller errors, meaning 
probably that the technique used to switch arrays can be 
improved. 

An interesting point is the behavior of the curves for 
the diagonal terms of the matrix PKK. The curves PKK_X vs 
TIME_SLOT, PKK_Y vs TIME_SLOT and PKK_Z vs TIME_SLOT, which 
are the mean square errors should grow proportional ly to the 
square of the position errors. These are depicted in the 
curves X_ERR vs TIME_SLOT , Y_ERR vs TIME_SLOT , and Z_ERR vs 
TIME_SLOT. When such does not happen it means that the 
filter model used is not a correct description of the 
process. When the value of the diagonal elements of the 

— 8 

matrix R were 10 , only the qualitative behavior of the 

— 1 o 

mean square errors agreed with the expected. When 10 was 
used, the quantitative behavior of the mean square errors 
were about the expected. 

Additional tests should be performed using trajectories 
generated from actual torpedo runs on the Dabob test range. 
As a future step, there is the need to adapt the program to 
provide it with the correct interface formats to access the 
real data, before running it in the real-time conditions on 
the range. 
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APPENDIX A 



PROGRAM DESCRIPTION 

A. OVERVIEW 

There are at this moment, three versions of the Extended 
Kalman Filter program. In either case the simulated 
trajectory must be first computed and the hydrophones array 
times, corresponding to the simulated positions, are stored 
in a file in order to be read in. All three programs write 
the files XKK.DAT, PKK.DAT, and XDIFF.DAT to the disk. 

To avoid delays due to data writing and retrieving from 
disk drives, the data files can be written to and read from 
an electronic drive built in the RAM memory. There is also a 
utility program - PLQT.C - which allows fast graphic 
analysis after the run. 

1 . The CIE1.C Program 

This was the first program to run in the CIE 
mi crocomputer . It reads "clean" (without noise) trajectory 
data from the file TRAJ.DAT and writes 3 columns, 
corresponding to the coordinates x, y, and z, to the disk. 
The code to generate Gausian noise is contained in the main 
program. 

CIE1.C generates 3 data files, XKK.DAT, PKK.DAT, and 
XDIFF.DAT, containing filtered data to be post-processed. 
XKK.DAT, contains 4 columns, correspondi ng to the time slot 
and the filter estimates, X(k/k), for the coordinates x, y, 
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and z in that order. PKK.DAT contains 4 columns with the 



correspondi ng values of the time slot and the error 
covariance matrix for the coordinates x, y, and z, in that 
order. XDIFF.DAT contains 4 columns with the correspondi ng 
values of the time slot and the errors for the coordinates, 
x, y, and z, in that order. 

2. The CIEKR1.C Program 

This program runs in the CIE and provides input and 
output similar to the KR.C . It is able to simulate only 
straight line trajectories, due to the lack of an "arc tan" 
function in the CIE's C Compiler, and at present does not 
access the computer internal clock. 

3. The KR.C Program 

This program includes a set of assembly language 
routines for interfacing with the keyboard, display, and 
output ports of the IBM-PC/AT. It also includes a plotting 
routine, written in C, which calls the assembly routines to 
generate the frame, to print updated values, and to draw the 
estimated trajectory. Another assembly language routine was 
added to allow access to the internal clock in the IBM-PC. 
See references C 71 and C81 and Appendix C for details. 

The Kalman Filter section of KR.C needs the files 
TRJ.DAT and ZIC.DAT, generated by. the trajectory simulation 
routine, for inputs. 

The trajectory simulation generates transit times 
data already corrupted with noise. That involves inclusion 



123 



of the noise generation routine with the "clean" trajectory. 
The clean trajectory can be either a straight line or a 
circular curve. 

The trajectory simulation routine creates the files 
TRJ.DAT, ZIC.DAT, TRJ. INP, and PAR AM. DAT . 

The TRJ.DAT contains the data for the clean 
trajectory, corresponding to the time slot, and the x, y and 
z coordinates. ZIC.DAT contains the noise corrupted transit 
times. TRJ. INP and PARAM.DAT contain information to be used 
by PLOT.C program in order to identify and relate the 
trajectory data and the parameters used in the Kalman Filter 
to the graphic outputs. 

B. DETAILED DISCUSSION 

Since the programs evolved one from the other, only the 
last one, KR.C, will be discussed in more detail. The 
routine that generates the trajectory and the tools 
developed to facilitate the objective analysis of results 
will also be briefly described. 

1 . The Kalman Filter Program - KR.C 

The algorithm for the program KR.C is provided as 
Appendix B and the actual program implementation is shown in 
the Appendix E. 

The capability of .tracking trough multiple arrays is 
implemented by simply defining switching points along a 
coordinate axis, and comparing the estimated x-position 
against switching values. The coordinates of the tracking 
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array are then changed in order to always keep the closest 
array tracking the torpedo. Figure 90 shows the coordinate 
system for the multiple array tracking, the location of the 
switching points and the hydrophone location matrix. 

Tests performed using a straight line trajectory and 
keeping the Q matrix constant showed no improvement over the 
case where the adaptive Q is used. 

2. The Trajectory Simulation Routine - TRAJEC 

This routine was developed as an independent 
simulation program, to generate tra jector i es , where circular 
and linear segments can be combined to produce a desired 
path. 

Given the initial x,y,z, positions and the initial 
speeds and assuming that the vertical speed is zero, this 
routine generates noisy data to represent the "raw" data as 
it would come from the hydrophones. TRAJEC finds the 
position of a point in a circle after a time interval T, 
given the tangential speed of the point, and given the 
desired turn rate or given the centrifugal acceleration. It 
is assumed that the tangential speed is constant during the 
maneuver. Either the turn rate or the centrifugal 
acceleration can be used to determine the radius. 

Figure 91 represents the geometry of the problem. The 
straight line shown is tangent to circle. A point is 
initially moving on the line and at the time t Q it is 
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Figure 91 i The circular trajectory problem. 
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subjected to centrifugal force which makes it turn. The 
point then describes a circular movement generating a circle 
of radius R, and at the time t Q +T will be at the position 

X.Y. Assume V and V known speeds, m = V /V is then the 
’ y x ’ y x 

slope of the line tangent to the circle that passes at the 

point x 0 ,y Q and has center at a,b. The angular coefficient 

of the line perpendi cul ar to the circle and passing at the 

point x 0 ,y Q is 

<x 0 - a) / (y Q - b) = - 1 / m , so 

x „ - a = m * <y - b) 

0 o 

and R 2 - (x - a) 2 + <y - b> 2 is valid for any point 

on the circle. 

at x 0 * V 0 -» R 2 = <x 0 “ a> 2 + <y 0 - b) 2 
R 2 = (m 2 - 1) * (y - b) 2 
y Q - b = ± R / sqrt (1 + m 2 ) 

or b = y Q + R / sqrt (1 + m 2 ) 

and a = x„ + m * (y„ - b) 

0 '0 

For any point on the circle 

x - a = R cos e 
y - b = R sin 0 
From the Figure 91 
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oc = tan m 
© 0 = it / 2 - ot 

9 — 0 Q + a) A t 

Given V , V -> V. = sqrt (V 2 + V 2 ) 
x ’ y t x y 

Given u -» R = V^. / w 

Given the central acceleration, g 4 u = g / 

At the end of the interval, the position will be x, 
y and the speeds will be given by 

V = - V, sin 9 

X XL 

V = V cos 9 

y t 

These equations, when implemented, generate the true 
tra jectory . 

The routine also computes the measurement noise 
times. The Gaussian noise is generated by computing the 
linear approximation of the inverse of the normal curve C91. 
The input comes from a sequence of uniformly distributed 
numbers generated by the equation below 

r ( I + 1 ) ■ r ( i ) * 317. mod ( 1 ) . 

The resultant sequence is zero mean and is scaled to obtain 
the desired variance. 

3. The Plottin g Routine - PLOT 

The plotting routine is contained in the file PLOT.C 
(IBM-PC), and was written as an independent module to be 
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and reads data -from the -files 



linked with BIOSIQ.OBJ, 

XKK.DAT, PKK.DAT, XDIFF.DAT, TRJ.INP, and TRJ.DAT. 

That is a menu driven program, which provides 
fourteen (14) different options and was designed to simplify 
the analysis of data generated in a torpedo run. The plots 
have their titles already chosen. The scales reflect the 
maximum and minimum values. A reference number and a label 
for SINGLE or MULTIPLE array cases is printed on the plot 
for reference. Linear interpolation is used to interconnect 
the sample data points. An optional plot, where the data 
points are simply plotted, is also available. This last 
option would allow the time to be associated to each point 
on the curve. 

In an attempt to provide the CIE machine with 
similar capabilities, an ANSI driver was also defined and 
tested successfully in the IBM. However, that provides a 
very limited graphics capability, allowing only 80 columns 
by 25 rows of resolution against the 640 by 200 in the IBM, 
in graphics mode. The file KR1.C makes use of the defined 
driver to produce the graphics presentation of the estimated 
tra jectory . 

For UNIX systems, like the CIE, one might want to 
consider the "termcap" database of hundreds of encoded 
descriptions for terminal control . However, except in 
unusual ci rcumstances , C's standard-1 i brary I/O functions 
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provide excellent portability and a wide range of display 
-features C63 . 

4. B I OS IQ. ASM and BIQ5IQ.QBJ 

These -files contain the assembly language code 
(public domain) and the object code (IBM-PC) to inter-face 
the C programs with the keyboard, display, output ports and 
to access an absolute memory location. 

5. DOSTIME. ASM and DOST I WE. OBJ 

These -files contain the assembly language code and 
the object code, respectively, that allow access to the 
internal clock (IBM-PC). 

6 . The SIMTERM program 

This is a public domain program written by Jim & 
Eric Holtman that allows one to emulate an HP-like terminal 
to a UNIX system. It is a sel-f contained module that allows 
one to connect the IBM-PC to the CIE microcomputer , directly 
by using the RS-232 output on the IBM and one o-f -four (4) 
output ports on the CIE. 

7. The KALMAN. H header file 

It contains the de-finitions o-f the program 
constants, initial values o-f parameters, and the definition 
o-f the ANSI terminal driver, which can be used in the -future 
to generate rough plots on most of the nongraphic terminals. 



131 



APPENDIX B 



ALGORITHM FOR 



THE KR.C PROGRAM 



ttdefine CONSTANTS 
#def ine MACROS 
ttinclude FILE.H 
double xkkC2001; 
mai n ( ) 

£ 

/* DEFINING FILES */ 



/* used to define constants */ 
/* define macros */ 

/* include header file */ 

/* declare global variables */ 
/* main prog, declaration */ 

/* block/function delimiter */ 



FILE *fpxyz, ♦fopenO; /* declares fpxyz as a */ 

. /* pointer to a file */ 

. /* yet to be open using*/ 

/* fopenO function */ 



/* DECLARATION OF VARIABLES */ 



i nt i , j ; /* 
short i i , j j ; /* 
float x,y; /* 
double xx, yy; /* 
/* OPENING FILES */ 



long integers */ 
short integers */ 
single precision */ 
double prec. reals */ 



f pp=f open < "pkk. dat " , "w" ) ; /* open file pkk.dat */ 

/* for writing */ 

fpe=f open < "xdiff . dat ", "w" ) ; /* open xdiff.dat */ 
fpxyz=f open < "x kk. dat ", "w" ) ; /* open xkk.dat */ 
/* INITIALIZATION OF VARIABLES */ 

Initialize HYDRO C D 
Initialize gammaCl 
Initialize covwC] 

Initialize xkkmlCl 
Initialize xb Cl , yb C 1 , zb C 1 



/* DATA 


INPUT - 


INTERACTIVE */ 


menu: Put 


menu 




Get 


i nputs 






Perf orms 


actions 




Case 


statement 



1 Run trajectory 

Write run# and xkkmlCl to trj.inp 
Call trajec to read trj.inp and 
generate noisy trajectory 

2 Run Kalman filter 

3 Modify Parameters 

a. Optimization criteria 

b. Single/multiple array 

c. Initial x, y, z 

d. Initial Vx , Vy 

e. Noise covariance 
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f. Error covariance 

g. Rede-fine maneuver 

- Turn rate based 

- G number 

- Start/end time 

h. Main menu 
/* Run Kalman filter */ 

run: Get run # for reference 

Save inputted data into files 

Open files trj.dat and zic.dat for reading 

Initialize QFIND block 

Initialize hrowCl 

Initialize pdumCj 

Initialize rrLl 

Prepare screen for real time presentation 
Calculate scaling 
Draw frame 

Get time from internal clock 
/* START TIME LOOP */ 

Start time loop : for < kk=0; kk< i ti me; kk++) 

Get hydrophone array coordinates 
again: Get time 

If time interval < 1.31 goto- again 
Update time 

Get true times e true positions (noisy data) 
Read data!] from file trj.dat 
Read zicCl from file zic.dat 
Save true positions in trueCD 
adapt: /* GET HROW, CALCULATE GAIN, ESTIMATE */ 

/* COVARIANCE OF ERROR SEQUENCIALLY * 
js: for <i =0; i <4; i++) 

•C 

Calculate denom using double prec. sqrt 
Calculate hrowCOl, hrowC23, hrowC41 
Calculate gnumCl, gdenom 
Calculate gill = gain column 
Calculate covariance of error piC] 
for (i i=0; i i<25; ii++) 

calculate pdumCl 

> 

calculate pi C 3 

Calculate measurement prediction 
calculate zhat=denom/4860. 
calculate zdif f Ci 3=zicCi 3-zhat 
Compute 3sigma gate 

p=max (piC01,piC121,piC241) 

/* use single precision sqrt */ 
gate=3. * ( sqrt (p ) /4860 + rrCil) 

Edit invalid time measurements 
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Set -flag 

Put gain, giC3=0 

Calculate estimate based on one meas. 

predi ct i on 

■for (ii=0;ii<5jii++) 

xiCii3=xkkmlCii3+giCii3*zdiffCi3 
/* Update for next column */ 
for <ii=0;ii<5;ii++) 

xkkmlCii 3 =x i Cii 3 
pkkmlCii 3=pi Cii 3 

y 

for (i i =5; i i <25; i i++) 
pkkml Cii 3=pi Ci i 3 
if <i==3) goto 156 
} /* end of js loop */ 

/■* Accept values as updates for time */ 

/* interval */ 

156s Do updates for the time interval 

xkkml C 3=xi C 3 
pkkml C 3=pi C 3 
xkkC3=xiC3 
pkk C 3=pi C3 

! Recalculate time measurements and form 

(1) ! absolute value of residuals 

! Edit invalid times for adaptive maneuver 
! routine 

Compute adaptive Q matrix 

If <icurve==l) compute adaptive Q 
else use constant Q 
! Check time 

! By pass maneuver if no time left 

! Check for valid measurements 

! By pass adapt, maneuver if all invalid 

! By pass maneuver if not steady state 

(2) ! Check average zdiffCil vs. zdifav 

! By pass maneuver if avg.<zdifav 

I /* Adaptive maneuver */ 

i pkkml C 3=pkk C 3+q C 3 

! /* Reiterate same time slot */ 

! goto adapt 

Compute measurement errors 

xdif f C0 3=xkkC0 3-truedC03 
xdiff C13=xkkC23-truedC13 
xdif f C23=xkkC43 -trued C 4 3 
! Compute switch to change between Q 

(3) ! constant : straight line trajectory 

! or Q adaptive : circular trajectory 

Calculate predictions for the next time 
pkkmlC3 and xkkmlC3 
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Output results to the screen 
x ,y , z , vx , vy, time values 
plot xkkCOl, xkkC21 
Output results to -Files 
xkkC3,pkkC3,xdi-F-FCl 
> /* end time loop */ 

Close -Files 
> /* end mainO */ 
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APPENDIX C 



SETTING UP FOR EXECUTION 

A. USER'S NEEDS 

This section describes the actual hardware and so-ftware 
utilized to produce the results in Appendix H. 

HARDWARE 

The -following describes the hardware utilized and the 
way to interconnect the IBM-PC and the CIE mi crocomputer . 

IBM-Pc : 2 -floppy disk drives, 10 megabytes hard disk, 

graphic board, graphic color display, serial port (RS-232), 
S0S7 math coprocessor chip and Epson parallel printer; 

CIE 680/30 : 20 megabytes hard disk, 4 serial output 

ports, Amdek monitor and keyboard. 

The computers were interconnected by cable, from the IBM 
RS-232 output port to one o-f the 4 ports on the CIE. 

SOFTWARE 

The -files and/or programs utilized ares 

1. SIMTERM - communications program (in the IBM). Allows 
transferring -files between the IBM and the CIE. 

2. CIE. BAT - batch -file (IBM). Executes the necessary 
sequence to interconnect the IBM and the CIE. Needs SIMTERM. 

3. KR.EXE - executable -file that contains the program to 
simulate the torpedo trajectory and the Kalman -filter 
program (IBM). The trajectory routine is executed directly 
•from the KR.C program as a menu option and it generates the 



136 



data -files "trj.dat", and "zic.dat". Then the trajectory and 
time in-formation -from the -files, "trj.dat" and "zic.dat", is 
used by the Kalman -filter algorithm to produce estimates. It 
generates the -files "xkk.dat", "pkk.dat", "xdi -f -f . dat " , 
"trj.inp" and "param.dat". At the end o-f the run, a total o-f 
7 -files would be generated. 

4. PLOT.EXE - executable -file that contains the plotting 
routine (IBM). Allows posterior graphic analysis o-f the 
results. Uses the in-formation contained in the -files 
"xkk.dat", "pkk. dat " , "xdi -f -f . dat " , " tr j . dat " and "trj.inp". 

5. KAL.BAT - batch -file (IBM). Executes the necessary 
sequence to run KR.EXE and PLCT.EXE. 

6 . KALMAN. H - header -file (IBM and CIE) . Contains 
de-finitions used by CIEKR1.C , KR.EXE, PLOT.EXE, TR. EXE . 

7. a. out - executable -file generated -from compilation o-f 
CIEKR1.C on the CIE machine. 

B. PROGRAMMER'S NEEDS 

It is highly recommended to have a good -full-screen text 
editor, and the source code files included in the directory 
of the IBM-PC. Source code files have extension " .C". A good 
C compiler (IBM) is also recommended. 

The software required is: 

1. LATTICE C or MICROSOFT C compiler ; 

2. FULL SCREEN EDITOR ; 

3. SIMTERM - terminal emulation / communication program: 
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4. SOURCE -files s TR.C, KR.C, PLOT.C ; 

5. HEADER -files : KALMAN. H, MATH. H, STDIO.H ; 

6. ASSEMBLY LANGUAGE ROUTINES: BIOSIO.ASM, DOSTIME. ASM ; 

7. IBM MACRO ASSEMBLER ; 

S. MICROSOFT LINKER . 

Using the Lattice C compiler requires the files LC1.EXE, 
LC2.EXE, LCS.LIB, LCMS.LIB, CS.OBJ, MATH. H , STDIO.H to be 
present. The assembly language routines must first be 
transformed into object files by using the Macro Assembler. 
The file TR.C must also be compiled using LC1 and then LC2 
to generate the TR.OBJ file. These are all linked to the 
file KR.OBJ to produce the executable file KR.EXE . 
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APPENDIX D 



COMPILING, EXECUTING, TRANSFERRING FILES, AND PLOTTING 
A. RUNNING THE PROGRAM ON THE IBM-PC 

This will assume one is running the Kalman Filter 
program on the IBM PC/AT with a hard disk drive defined as 
drive "c".. The system prompt is "c>", and the required 
-files are in the subdirectory named "KALMAN". 

If you are not in drive "c", change to the correct drive 
typing "c:" and hit "RETURN". The system will then answer 
with the prompt "c>". Type "cd kalman" to access the 
directory KALMAN . 

1 . Compiling KR.C 

This will assume you are running the Lattice C 
compiler. Type "ce kr" to compile the file KR.C, using the 
batch file "CE. BAT" . The files KALMAN. H, DOSTIME. OBJ, 
TR.OBJ, B IOSS 10. OBJ , LINK.EXE, CE. BAT , and KR.C need to be 
in the same directory. 

The Appendix E contains a listing of the file 

CE. BAT. 

2. Executing KR.EXE and PLQT.EXE 

Type "KAL" ,in drive "c", to execute the batch file 
KAL.BAT. The required sequence of commands is initiated and 
can be interrupted by pressing "CTRL-BREAK" ("CTRL" and 
"BREAK" keys at the same time). The Appendix E contains a 
listing of the file KAL.BAT . 
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The first file to be executed is GRAPHICS. It allows 



dumping IBM-PC graphic screen to the printer. 



Next 


the 


file KR. EXE, 


containing 


the trajectory 


and 


the 


Kalman Filter 


program, is 


executed. Menus 



give the indication for data input, to run the trajectory 
routine, and to run the Kalman Filter. One normally defines 
the parameters first (option # 3) , then run the trajectory 
routine (option # 1), and then run the Kalman Filter (option 
# 2 ). 

After the run, the plotting routine ("Plot") is 
called to produce the graphic output. Up to fourteen 
different plots can be generated by making the desired 
selection from the menu. The output to the printer is 
obtained by pressing "SHIFT-PRTSC" ("Shift" and "PrtSc" at 
the same time) . 

If a plot ends abnormally, for instance by the use 
of CTRL-BREAK, the screen is left in the graphics 640 by 
200 mode, then the command "mode bw80" will set the display 
mode to Black and White with 80 characters per line. 

B. RUNNING THE PROGRAM ON THE CIE 

Turn on the system with the floppy disk drive open. This 
will force the system to boot and accept the internal hard 
disk drive as the current drive. Turn on the terminal 
display and when the cursor appears, press "ENTER". Follow 
the prompts for date and time input. 
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After booting, the screen will show the message "CIE 
GRAPHICS SYSTEM # 7" at the top, and "login : " at the 
bottom. At this moment one should have the SIMTERM diskette 
in drive A (the one at the left) on the IBM-PC. If the 
logged drive is not A, one should type "a: 11 and press 
"ENTER" to change to drive A . In drive A, one should type 
"cie" and "RETURN" (ENTER). The batch file CIE. BAT will send 
the correct command line to connect the IBM and the CIE. 
Make sure the cable connection is already done. 

The message "connection established" will then appear on 
the screen. Press "ENTER" to get "login:". Log in using the 
account "jose", and press "ENTER" for the password, the 
system prompt is "V." . Change directory using "cd work" . 
One is then ready to transfer files or to execute a program 
in the CIE machine. 

1 . Transf erring Files from the IBM to the CIE 

To transfer a file use the redirection capability of 
the Unix system and the command "cat". 

Type "cat > f i 1 ename. ext " and press "ENTER", at the 
system prompt ("%"). The cursor will stop at the begining 
of the next line. "ALT-D" will dump a file to the 
communi cati on line. One will be prompted for the filename. 

If, for instance one wishes to transfer the file 
"kalman.h", which is in drive c: (IBM), to the CIE, given it 
the name "kalman.h", the following sequence should be 
performed, starting at the prompt : Type "cat > 
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the name "kalman.h" , the -following sequence should be 

per-formed, starting at the prompt "/£" : Type "cat > 

kalman.h"; press "ALT-D"; type "cskalman.h" , when asked -for 
the IBM filename, and press "ENTER" to start the 

transmission. 

At the end of the transmission the computer bell 
will ring, signaling buffer overflow. One should wait until 
no more new lines are being written. Press "CTRL-D" to close 
the file in the CIE. The prompt "5i" will reappear on the 
screen . 

The new file (kalman.h) will have to be edited on 
the CIE to remove the inserted empty lines: 

Type "ed kalman.h" and the editor will show the 
number of lines in the file. Then, type "g/' s $/d" and press 
"ENTER". The number of lines in the file will be reduced to 
the half. This will not affect the blank lines originally 
in the program, just the ones inserted by the process of 
transmission. 

Type "w" to write the modified program version to the 
disk, and "q" (quit) to leave the editor. 

2. Compiling on the CIE 

Type the command line "cc -f -v -L -7 ciekrl.c" to 
generate the executable file "a. out", 
where : "cc" invokes the C compiler; 

"~f" option , informs the compiler to use "fast 
floating point format"; 
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M -v" option ... 

L" option instructs the compiler to generate 
pointers to allow long jumps, -for a code+data greater 
than 32 kbytes; 

"-7" option instructs the compiler to use the Unix 
version 7 standard library. 

3. Executing CIEKR1 

Type "a. out" to start execution and then -follow the 
menus -for inputs. Since the CIEKR1.C program does not 
generate circular trajectories , the -files containing the 
trajectory data (TRJ.DAT) and transit time data (ZIC.DAT) 
will have to be transferred from the IBM to the CIE, in 
order to run the Kalman Filter for the circular trajectory. 
Be sure to input the same data that were used to generate 
the circular trajectory on the IBM, and then select option 
#1 (Run Kalman). For straight line tra jector i es , just follow 
the menus. 

4. Plotting the Results 

Since there is no graphics capability built in the 
CIE, the only way to obtain plots is to transfer the files 
XKK.DAT, PKK.DAT and XDIFF.DAT, obtained on the CIE, back to 
the IBM and then use the routine PLOT to generate the 
desired graphic output. The procedure is described below: 

"ALT-C" , during the SIMTERM session, will open a 
capture file. One will be asked the name of the capture, or 
log file. The drive name should be specified. 
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For instance, assume one wants to transfer the file 
XKK.DAT from the CIE to the IBM in order to use it later to 
generate plots. Since, the routine PLOT reads specific 
files, be sure to erase the old file XKK.DAT, in the IBM, 
before copying the file XKK.DAT from the CIE, because the 
same name will be used. 

Type "list -n xkk.dat", at the prompt "V ." , without 
pressing "ENTER". Pressing "ALT-C" , will start the capture 
mode. Then one will be asked the name of the log file. Type 
"csxkk.dat", and press "ENTER", to open the file on drive C. 
The command line "list -n ... " will reappear. Press 
"ENTER" to start the transfer and the capture. Press "ALT- 
C"at the end of the file listing, to close the log file. 

After doing similar procedure for the files 
XDIFF.DAT and PKK.DAT, leave the account on the CIE by 
typing "logout". 

Leave the SIMTERM program and drop the line, 
pressing "ALT-F2". One will be back to drive A on the IBM. 

Type "c:", to change logged drive to C . 

One may use his preferred text editor to eliminate 
the undesired lines in the files XKK.DAT, XDIFF.DAT, and 
PKK.DAT. The files should contain only four columns of 
numbers, except for the first row, which is blank. Then one 
can use the routine PLOT to obtain the desired plots, by 
typing "PLOT =8000", and following the menu options. 
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APPENDIX E 



KR.C PROGRAM LISTING - IBM-PC VERSION 
This Appendix contains a listing o-f the Extended Kalman 
Filter program implemented in the IBM-PC mi crocomputer , as 
well as the listings o-f the Trajectory Simulation routine 
and the header -file "kalman.h" , which are used by the main 
program KR.C . The other header -files declared at the 
begining o-f the main program are standard -files contained in 
the C compilers. 
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/**********************************************************/ 
/* */ 

/* HEADER FILE - KALMAN. H */ 

/* */ 

/*********************************************************** 



#de-fine nmax 5 /* 

#define maxsq 25 /* 

#de-fine js 4 /* 

/* 

#define hcmax 12 /* 

/* 

#de-fine hmax 72 /* 

#de-fine dmax 3 
#de-fine itime 150 /* 

#define npt 150 
#de-fine icmax 5 /* 

/* 

#de-fine criter 0.1 /* 

#de-fine na 0 /* 

/* 

#de-fine xpos 10000.0 /* 

#de-Fine xvel -50.0 /* 

#de-fine ypos 0.0 /* 

#de-fine yvel 5.0 /* 

#de-fine zpos 0.0 /* 

#de-fine swO 33000.0 /* 

#de-f ine swl 27000.0 /* 

#de-f ine sw2 21000.0 /* 

#de-fine sw3 15000.0 /* 

#de-fine sw4 9000.0 /* 

#de-fine sigdiv 1.0 
#de-f ine see 0.387463094 /* 
#de-fine sacc 36.2 
#de-fine vel 4860.0 /* 



#define twopi 6.28315308 
#de-fine rrvar l.e-5 
#de-fine rvar l.e-10 
#de-fine pkkdiag 1000.0 
#de-fine limvar 1.5 

/* 

#define gmO 0.86 

#de-fine gml 1.31 

#de-fine cwO 1.0 /* 

#define cwl 1.0 /* 

#de-fine cw2 1.0 

#de-fine xminl -5000.0 
#de-fine xminh 0.0 
#de-fine xmaxl 5000.0 
#de-fine xmaxh 40000.0 



# of col. in pkk, q ... */ 

size o-f equiv. vector PKK,Q...*/ 

# hydroph. in any hydrophone */ 
array */ 

# of col. in matrix HYDRO of */ 
hydroph. coord. */ 

equiv. vector size of HYDRO */ 

max.# of time slots */ 

max.# of optimal iterations */ 
allowed */ 

criteria to compare ZDIFAV •*/ 
may assume ONLY values 0 or 6 */ 

0 =>single;6=> multiple arrays */ 
initial torpedo x-coordinate */ 
initial torpedo x-velocity */ 
initial torpedo y-position */ 
initial torpedo y-velocity */ 
initial torpedo z-position */ 



swi de-fine points where ■*/ 

switching -from one to */ 

another */ 

array */ 

is done. */ 

- in radians. . . */ 

speed o-f sound */ 



/* de-fines threshold to use Q*/ 
constant */ 



initial was 1036.84 */ 
initial was 1036.84 */ 
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#de-fine yminl -5000.0 
#de-fine yminh -5000.0 
#de-f i n e ymax 1 5000 . 0 

#de-fine ymaxh 5000.0 



/* */ 

/* DEFINE ANSI DRIVER */ 

/* */ 

/■*********************************************************** 



/* #de-f ine STD IN 0 */ 

/* #def i ne STDOUT 1 */ 

/* #de-fi ne STDERR 2 */ 

#de-fine CLR_SCRN -fputs ( "\33C2J" , stderr) 

#d e-fine CUR_MV (row, col ) -fprint-f (stderr , "\33C7.d; */.dH" , row, 
col ) 

#de-fine CUR_UP(num) -fpri nt-f (stderr ," \33C7dA" , num) 

#de-fine CUR_DN(num) -fpri nt-f (stderr ," \33C7dB" , num) 

#de-fine CUR_RT (num) -fprint-f (stderr , "\33C%dC" , num) 
ttdefine CUR_LT (num) -fpri nt-f (stderr ," \33C%dD" , num) 

#d e-fine CUR_SKIP -f puts ( " \n " , stderr) 

#de-f ine CUR_SAVE -f puts < " \33Cs" , stderr) 

#de-f i ne CUR_REST -f puts ( " \33Cu" , stderr) 

#d e-fine CLR_LINE -f puts < " \33CK" , stderr) 

#define BELL -fputc('\7', stderr) 

#de-f ine CLR_E0S(r, c) Cbyte i_j CUR_MV (r , c) ; \ 

■for ( i _=r ; i_<=25;++i_) CLR_LINE, CUR_DN (1); 

CUR_MV (r,c); > 
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/**********************************************************/ 
/* */ 

/* REAL TIME KALMAN FILTER PROGRAM - KR.C */ 

/ * */ 

/*********************************************************** 

ttinclude "stdio.h" 

#include "math.h" 

#include "kalman.h" 
double 

xkkml Cnmax 3 , pkkml Cmaxsq 3 , pkk Cmaxsq 3 , xkkCnmax 3 , dataCdmax 3 ; 



/* STARTING THE MAIN PROGRAM... */ 

main <) 



/tt*#*******************************##***#**************#***/ 
/* */ 
/* VARIABLE DECLARATIONS */ 
/* */ 
/******************■*************•**************•****■*■*•*•**■#■***■* 



FILE *fpxyz, *-fopen(>; 
FILE *-fpe, *-fopen<>; 
FILE *fpp, *-fopen(); 
FILE *fptrj, *fopen(>; 
FILE *fpzic, *-fopen<>; 
FILE *-fparam, ♦fopenO; 
FILE *-fpin, *fopen(>; 



int ihours, imins, isecs , i ths ,col ,row,maxopt; 

int i 8, i , j , kk , i in , icont , i curve , option ; 

int param, tr jopt ,nrange; 

int i i , j j , narray , nzdi-f t , in , pli ,runn , c; 

int jstart , jend , worg; 

float rabs,zgate; 

double height , wi dth ,ymax , ymin ,xmax , xmin; 
double xscal e , yscal e , bx , by ; 

double hours, mi ns, secs ,ths,time,oldtime,deltat; 

double sqrt ( ) , ol dx , val ue , xO , yO , zO , vxO , vyO; 

double denom,al4,gdenom,gate,zdif av; 

double den , vel m, temp; 

double a2,gl ,g2,g3,pl ,xdiff C33 ; 

double zhat , zcs, dk , sigcc , si gacc; 

double v,truedC33 ,wd,g; 

double x , y , z , auxh , xd , yd , zd; 

double al , el , el 2 , e2 , bb , dd , bd ; 

double pkl,pk3,pk5,pgate,rgate,p,pll; 

doubl e phi pkk Cmaxsq 3 , pktemp Cmaxsq 3 , q Cmaxsq 3 ; 
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double 

double 

double 

double 

double 



pi Cmaxsq3 , pdumCmaxsq3 ,qtemp Cmaxsq3 ; 

gnumCnmax3 ,gi Cnmax3 ,xi Cnmax3 ,hrowCnmax3; 

zicCjs3,ziCjs3,zdi-f , fCjs3,rEjs3,phisCjs3,ztEjs3; 

xbCjs3,ybEjs3,zbCjs3,rrCjs3; 

hydro Ch max 3 , covwC33 , gamma C 23 , opt C53 ; 



/*********************************************#************/ 
/* */ 

/* INITIALIZATIONS */ 

/ * #/ 

/*****************************hh****************************/ 



f pxyz=-fopen < "xkk. dat " , "w" ) ; 
f pe=f open ( "xdi-f-f . dat" , "w" ) ; 

■f pp=-f open ( "pkk . dat " , "w" ) ; 

/* INITIALIZE CONSTANTS, HYDROPHONE MATRIX, PHI, R, 
DATA FOR TRAJECTORY, AND LOAD X(0/-1), P(0/-1) 

*/ 



nzdi-f-f=4; icont=0 


; icurve=l; value 


=criter; wd=! 


5.730; 


runn=l; narray=na; 


param=0; worg=l; 


g=0. 156; nrange=0; 


jstart=40; jend=SO 


; i8=0; max opt 


=icmax ; 




hydroC0]=36000. 0; 


hydroC 13= 6000.0; 


hydroC23= 


0. 0; 


hydroC3D=36030. 0; 


hydroC43= 6000.0; 


hydroC53= 


0.0; 


hydro C 6 1 =36000. 0; 


hydroC73= 6030.0; 


hydroC83= 


0. 0; 


hydroC9]=36000. 0; 


hydroC 103=6000. 0; 


hydroC 11 3= 


30.0; 


hydroC 121=30000. 0; 


hydroC 133=6000.0; 


hydroC 143= 


0.0; 


hydroC 151=30030. 0; 


hydroC 163=6000. 0; 


hydroC 173= 


0.0; 


hydroC 183=30000. 0; 


hydroC 193=6030.0; 


hydroC203= 


0.0; 


hydroC21 3=30000.0; 


hydroC223=6000. 0; 


hydroC233= 


30.0; 


hydroC243=24000. 0; 


hydroC253=6000. 0; 


hydroC263= 


0.0; 


hydro [273=24030. 0; 


hydroC2S3=6000. 0; 


hydroC293= 


0. 0; 


hydroC303=24000. 0; 


hydroC31 3=6030. 0; 


hydroC323= 


0.0; 


hydroE333=24000. 0; 


hydroC343=6000. 0; 


hydroC353= 


30.0; 


hydroC363 = 18000. 0; 


hydroC373=6000. 0; 


hydroC383= 


0.0; 


h y dr o C 39 3 = 1 8030 . 0 ; 


hydroE403=6000. 0; 


hydroC41 3= 


0. 0; 


hydroC423 = 18000. 0; 


hydroC433=6030. 0; 


hydroC443= 


0.0; 


hydroC453=18000. 0; 


hydroC463=6000. 0; 


hydroC473= 


30.0; 


hydroC 483= 12000. 0; 


hydroC493=6000. 0; 


hydroC503= 


0.0; 


hydro C51 3=12030. 0; 


hydro C523=6000. 0; 


hydroC533= 


0.0; 


hydro C54 3= 12000. 0; 


hydroC 55 3 =6030. 0; 


hydroC563= 


0.0; 


hydroC573=12000. 0; 


hydroC583=6000. 0; 


hydroC593= 


30.0; 


hydroC603=6000. 0; 


hydroE61 3=6000. 0; 


hydroC623= 


0.0; 


hydroC633=6030. 0; 


hydroC643=6000. 0; 


hydroC653= 


0.0; 


hydroC663=6000. 0; 


hydroC673=6030. 0; 


hydroC683= 


0. 0; 
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hydroC693=6000. 0; hydroC703=6000. 0; hydroC713= 30.0 



rC03=rvar; rC13=rvar; rC23=rvar; rC33=rvar; 

gammaC03=gm0; gammaC 1 3=gml ; 
covwC03=cw0; covwC13=cwl; covwC23=cw2; 

x0*xpos; vx0=xvel; y0=ypos; vy0=yvel ; 
z0=zpos; 



for <i=0; i<nmax ; i++) 

for ( j=0; j<nmax ; j++) pkkml Ci *nmax+ j 3=0. 0; 
for < i =0; i <nmax ; i ++) pkkml Ci *nmax+i 3=pkkdi ag; 



phisC03=1.31; phisC13=0.0; phisC23=l . 31 ; phisC33=0.0; 



xbC03= (-15. 0) ; 
xbC 1 3= 15.0; 
xbC23= (-15. 0) ; 
xbC33= < — 15. 0) ; 



ybC03= (-15.0) ; 
yb C 1 3= < — 15. 0) ; 
ybC23= 15.0; 
yb C33= <-15. 0) ; 



zbC03= <-15. 0) ; 
zbC13=<-15.0) ; 
zbC23= <-15. 0) ; 
zb C33= 15.0; 



/* INITIALIZATION FOR QFIND */ 

velm=l . 0/vel ; 

al4=phi sC03 ; 

a2=al4*al4; 

gl=a2*0. 5; 

g2=gl*gl; 

g3=al4*gl ; 

si gacc=sacc*sacc ; 

sigcc=scc*scc; 

for < i =0; i <nmax ; i ++) 

for < j=0; j<nmax ; j++) qCi *nmax+ j 3=0. 0; 
qCmaxsq-l 3=si gdi v*a2*sigdi v; 

/* INITIALIZATION FOR HROW */ 
hrowC13=0.0; hrowC33=0.0; 



/* INITIALIZATION FOR PDUM */ 
for <i i =0; i i <nmax ; i i ++) 



j j=i i*nmax ; 
pdumC jj+1 3=0. 0; 
pdumC j j+33=0. 0; 

> 

pdumC&3=1.0; pdumC 183=1 . 0; 



/* INITIALIZING RR */ 

for (ii=0; ii<4; ii++) rr Ci i 3=sqrt (r Ci i 3 ) ; 
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/***************■»■»■»■»■»*■»■»*•*■*■**■»■»■»■*■»■»■»■»#**•»■»■»■»■»*■»■»*■»•»■»•»■»■»*■»*■»/ 
/* */ 

/* DATA INPUT */ 

/* */ 

/***********■»■»■»■»■»*■»■»■»■»■»■»■»■»■»*****■»■»■»■»■»■»■»■»■»■»■»■»***■»■»■»■»■»■»■»■»■»■»*■*/ 
restarts 

x kkml C03=x0; x kkml C 1 3=vxO; xkkmlC23=yO; xkkml C33=vyO; 
x kkml C43=zO; 



menus /* OPTIONS MENU */ 
printf ("Xn") ; 

printf ( "\n KALMAN FILTER ">; 

printf ("Xn") ; 



print-f ( "\n 
printf ("Nn" ) ; 
printf ("Xn 
printf <"\n"> ; 
printf ( "\n 
printf < "\n" ) ; 
printf <"\n 
printf ("Xn") ; 
printf ("Xn 
printf ("Xn") ; 
if <param!=0) 
i 



OPTIONS s"); 

1) RUN TRAJECTORY"); 

2) RUN KALMAN"); 

3) MODIFY PARAMETERS"); 

4) QUIT"); 



printf ("Xn") ; 

printf ("XnA parameter was changed ... You may "); 
printf ("have to run TRAJECTORY before continue."); 

y 

printf ("XnEnter value (1-4) s "); 
scanf ("Xd" , ^option) ; 
if (option==l) 



f pin=f open ( "tr j . inp" , "w" ) ; ' 
f printf (f pin, "Xd Xd" ,narray,runn) ; 

f printf (fpin, "XnXe Xe Xe", 

xkkml£03,xkkml£13,xkkmlC23) ; 
f printf (fpin, "XnXe Xe" ,xkkmlC33,xkkmlC43) ; 
f cl ose (fpin) ; 



tra jec (narray , hydro,xb , yb,zb,xkkml, j start , jend , wd , g , worg ) ; 
param=0; 
goto menu; 

> 

else if (option==2) goto run; 
else if (option==3) goto modify; 
else if (option==4) goto quit; 
else goto menu; 



151 



MODIFY PARAMETERS OPTIONS 



modi* ys 

print* <"\n 
print* ( "\n" ) ; 
print* ( "\n 



") 



1) ZDIFAV stop optimization"); 



print* (" criteria") 



print* <"\n 2) 
print* ( "\n 3) 
pri nt* ( "\n 4) 
print* ( "\n 5) 



print* <" R") ; 



SINGLE / MULTIPLE ARRAY"); 

INITIAL X, Y, and Z positions"); 
INITIAL Vx and Vy velocities"); 
NOISE COVARIANCE - diag_elem o* " ) 



print* ("\n 6) MAX. # ITERATIONS OF OPTIMAL); 

print* <" MANEUVER"); 

pri nt* ( "\n 7) REDEFINE MANEUVER - w / g / time) 

print* (" interval"); 



print* ( "\n 
print* ( " \n " ) ; 


8) 


USE LOW / HIGH RANGE "); 


print* ( "\n 
print* ( "\n" ) ; 


9) 


MAIN MENU - OPTIONS"); 


print* ( "\nEnter value (1-9) : "); 

scan* ( "7d" ,&param) ; 



swi tch (param) 

* 

case 1: 

print* ("\n '/. s 7.e" , "Current 

value) ; 

print* ( "\nchange ? CY/NI :"); 

scan* ("7.s" ,&c) ; 

i* (c== ' n ' !! c == ' N ' ) break; 

print* (" \nEnter new value (real) 

scan* ("7.1*" Rvalue) ; 

break ; 



CRITERIA 



"); 



is: 



case 2: 

pri nt* ( " \nSINGLE array is the de*ault"); 
i* (narray==0) 

pri nt* (" \n/is" , "Current case is : SINGLE"); 
else print* ( "\n7s" , "Current case is: MULTIPLE") 
pri nt* ( "\nchange ? CY/N3 :"); 
scan* ( "7.s" ,&c) ; 
i* (c== 'n ' !! c== ' N ' ) break; 

print* ("\nEnter S for SINGLE or"); 
print* (" M for MULTIPLE array : "); 

scan* ( "7s " , &c) ; 

i* (c== 's ' !! c=='S') narray=0; 

else i* (c== ' m ' !i c== ' M ' ) narray=l; 
else narray=0; 
break ; 
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case 3: 

printf ( "\nCurrent values are s "); 
printf <"\n7.s 7.e % s */.e 7.s •/.e","xpos = ",xkkmlCOD 
" ypos = ",xkkmlC2D," zpos = ",xkkmlC4D) 
print-f ( "\nchange ? CY/ND s"); 
scan-f ("7.s" ,&c) ; 
if (c== ' n ' ! ! c== ' N ' ) break; 

print-f ( "\nEnter new values : "); 

print-f ("\n7.s" , "xpos = "); 
scan-f ("7.1 f ",&x0) ; 

print-f ("\n7.s ,! , "ypos = "); 
scan-f ("7.1-f " ,&y0) ; 

print-f ( "\nXs" , "zpos = 
scan-f ("7.1f ",&z0) ; 

x kkml COD=xO; xkkmlC2D=yO; xkkml C4D=zO; 
break ; 
case 4s 

print-f ( "\nCurrent values are : 

print-f ("\n7.s 7.e 7.s 7.e","xvel = ",xkkmlClD, 

" yvel = " ,xkkmlC3D) ; 
printf ("\nchange ? CY/N1 «"); 
scan-f ( "7s" , &c) ; 
if (c== 'n ' ! ! c== ' N ' ) break; 

printf ( "\nEnter new values : " ) ; 

printf ( M \n7.s", "xvel = " ) ; 
scanf ( M 7.1f " ,&vxO) ; 

printf ("\n7.s", "yvel = "); 
scanf ( "7.1f " ,&vyO) ; 

xkkml C 1 3=vx0; xkkml C3D=vyO; 

break ; 

case 5: 

printf ( " \nCurrent values are s " ) ; 
printf ("\n7.s 7.e 7.s 7.e 7.s 7.e 7.s %e","rC 03 = ", 
r COD , " rCID = ",rClD," rC2D = ",rC2D, 

" r C3D = ", r C3D ) ; 
printf ("\nchange ? CY/ND 
scanf <"y.s",&c> ; 
if (c== ' n ' !! c== 'N ' ) break; 

printf ( "\nEnter new values : "); 

printf ("\n7.s" , "rCOD = " ) ; 
scanf ("7.1f ",&rC0D) ; 
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printf ( " \nXs" , "rdl 
scanf <"*/. If" , &r C 1 1 ) ; 

print-f <"\n'/.s","rC2] 
scanf <"7.1f " ,&rC23) ; 

print-f <"\n*/.s" , "r C31 
scanf < "71f " , &r C33 ) ; 

for <ii=0; ii<4; ii++) 
break ; 



"> ; 

rrCi i l=sqrt (rCi i 3 ) ; 



case 6s 

printf (" \nCurrent value is : "); 

printf ( "\n7.s 7d","# of iterations = ", max opt); 

pri ntf ( "Nnchange ? CY/N1 

scanf ("7.s" ,&c> ; 

if (c== ' n ' ! i c== ' N ' ) break; 

printf ("\nEnter new value : "); 

printf <"\n/is" , "maxopt # = "); 

scanf ( "%d" ,8<maxopt) ; 

break; 



turn 



case 7: 

trmenus printf ("\n 
printf ( "\n 
printf ( "on 
printf ("\n 
printf ( " on 
printf ( " \n 3) 

printf (" time slots") 
printf ( " \n 4) 

printf ("\n\nOption : 
scanf ("7d" ,S<tr jopt) ; 
if (trjopt==4) break; 
if <trjopt==l) 



DEFINE MANEUVER \n"); 

1) Define maneuver based"); 
rate" ) ; 

2) Define maneuver based"); 
g-number " ) ; 

Redefine start and end"); 



Modify 
"> ; 



menu" ) : 



printf ("\nCurrent value is : "); 

printf ("%s 7.e","turn rate : w = ",wd); 
worg=l ; 

printf ("\n Enter new turn rate (dg/s):") 

scanf ( "71 f " , 8<wd ) ; 

y 



else if (trjopt==2) 



printf ( "NnCurrent value is : ">; 

printf ("%s 7e" , "g-number : g = ",g); 
worg=0; 

printf ("\n Enter new g-number : "); 

scanf ("7.1f " ,&g> ; 

1 
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else if (trjopt==3) 

{ 

printf < "\nCurrent values are : "); 

printf < "\n/(s 7.d %s %d","t start = ", 

pri ntf (" jstart , "tend = ",jend); 
print-f ("\n Enter start time slot : ") 

scanf <"’/.d" jstart) ; 

print-f ("\n Enter end time slot : "); 

scanf ("’/.d" ,&jend) 5 

> 

goto trmenu; 
case S: 

pri nt-f ( "\nl_OW range is the de-fault"); 
print-f (" (use single array)"); 
i-f (nrange==0) 

print-f ( "NnXs" , "Current case is : LOW"); 
else printf ( "\n%s" , "Current case is : HIGH") 
pri nt-f (" \nchange ? CY/N3 :"); 
scan-f ( "’/Is" , &c) ; 
i-f <c== ' n ' !! c== ' N ' ) break; 

print-f ("NnEnter L -for LOW or"); 
print-f (" H -for HIGH range : " ) ; 

scan-f ("7.s" ,&c) ; 

if <c== ' 1 ' !! c=='L') nrange=0; 

else if (c== ' h ' !! c== ' H ' ) nrange=l; 

else nrange=0; 
break ; 

case 9: 

goto menu; 
break ; 

default : 
break; 

> 

goto modify; 

run: /* START EXECUTION */ 

printf ("\n") ; 

pr i ntf ( " \nEnter run # : "); 

scanf ( " V. d " , &runn ) ; 

f par am=f open ( "par am. dat " , "w" ) ; 

/* f printf (fpxyz , "X 2d" ,runn) ; 
fprintf (fpe, "72d" ,runn) ; 
f printf (f pp , "/12d" ,runn) ; 
fprintf (fparam, "72d" ,runn) ; 
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if (narray==0) 

£ 

f printf (fpxyz , "7.s" , " Single") ; 
f printf (fpe, " V.s " , " Single" > ; 
fprintf (fpp, "V.s" , " Single") ; 
fprintf (-f par am, "V.s" , " Single" ) ; 

> 

el se 
{ 

fprintf (-fpxyz ,"7.s" , " Mult") ; 
fprintf (-fpe, "7.s" , " Mult" ) ; 
f print-f (fpp, Mult"); 

fprintf (f par am, "V.s" , " Mult") ; 

> */ 

f print-f (-f par am, " \n V.s V.e " , "CRITERIA - value); 
-fprint-f (-fparam, "\n7.s V.e V.s V.e V.s V.e" , "xkkml C01 = ", 

xkkmlCOl," xkkml C 11 = ",xkkmlCll, 

" xkkml C21 = ",xkkmlC2D; 

-fprint-f (-fparam, "\n7s V.e V.s V.e" , "xkkml C33 = ",xkkmlC33, 
" xkkml C43 = ", xkkml C43 ) ; 

-fprint-f (-fparam, "\n7.s V.e V.s V.e V.s V.e V.s V.e ", 

" r C 0 3 = " , r C03 , " rC13 = ",rC13," rC23 = ", 

r C23 , " r C33 = ",rC33); 

-fprint-f (-fparam ," \n%s V.e V.s V.e V.s V.e" , "pkkml C03 = ", 

pkkml C03 , " pkkmlC13 = ",pkkmlC13, 

" pkkml C23 = ", pkkml C23 ) ; 
f close(f par am) ; 

f pin=-fopen ("tr j. inp" , "w") ; 

f print-f (f pi n , "V.d V.d" ,narray , runn) ; 
fprint-f (-fpin,"\n7.e V.e V.e" , xkkml C03 , x kkml C23 , 
xkkml C 43 ) ; /*posi ti ons*/ 

fprintf (fpin,"\n7.e V.e" , xkkml C 1 3 , xkkml C33 ) ; /*vel*/ 

f close (f pin) ; 
if (option==4) goto quit; 

f ptr j=f open ( "tr j. dat" , "r" ) ; 
f pzic=f open ( "zic. dat" , "r" ) ; 

wait: pr intf ( " \nPress g and ENTER to start ... " ) ; 

scanf ( "7s" ,&c) ; 



if 


(c ! = ' g ' && c 


! = ' G ' ) goto wait; 


if 


(nrange==0) 

f 






xmin=xminl ; 


xmax=xmaxl ; 




ymin=yminl ; 

y 


ymax=ymax 1 ; 
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el se 

i 

xmax-xmaxh; 

ymax=ymaxh; 

> 



xmin=xminh; 

ymin=yminh; 



hei ght=ymax-ymin; 
width=xmax-xmin; 
xscal e=600. O/wi dth ; 
yscale=-160. O/height; 
by=196. 0-ymin*yseal e+0. 5; 
bx=600. 0-x max *x seal e+0. 5; 

biosini (6) ; 

■frame (36,0, 196,600) ; 
biosset (23, 1 ) ; 

print-f ("7.. 1-f " ,xmin , " , " , ymin) ; 

biosset (6,61 ) ; 

print-f 1-f m /.sV.. 1-f " , xmax , " , " , ymax ) ; 
biosset (22,65) ; 
print-f ("7.s 7.d","# " ,runn) ; 
biosset (23,65) ; 

i-f (narray==0) print-f ( "SINGLE" ) ; 
else print-f ("MULTIPLE") ; 

print-f ("\nwait "); 



*/ 



/*************************•#■#**************■*■#•#*■***■#•#■*•#•*•*■#■#•**/ 
/* */ 

/* KALMAN FILTER */ 

/* */ 

/*#**####■##■#■****#*#■*####■#*##**#####■##**#*######*##■***#***#*/ 

/* START THE TIME SLOT LOOP AND SET ARRAY HANDOFF */ 

/* POINT */ 

■for (kk=0; kk<i time; kk++) 

i 

dk=kk+l ; 
do 

dost i me (&i hours ,&i mi ns ,&isecs , &i ths) ; 

hour s=i hours; mins=imins; secs=isecs; ths=iths; 

time=3600. *hours+60. *mi ns+secs+0. 01*ths; 

deltat=time-oldtime; 

> 

while (del tat < 1.25 && kk>0) ; 



oldtime=time; 

i-f (del tat >1 . 31 ) print-f ("*") ; 
else print-f (".") ; 
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/* 

/* 

/* 

/* 



printf ("\nXs %e","t = ", oldtime ); */ 
printf ( "7s 7.d" , " kk = ",kk>; */ 

printf ( "\n*********************************** M ) ; */ 
printf ( "720s 7.d \n" , "*•»*■***•»•»**•*■»* TIME ,kk); */ 



/* 

if 



GET HYDROPHONE ARRAY COORDINATES */ 
(narray != 0) 
f 

if (xkkmlC03 > swO) i8=0} 
else if (xkkmlC03 > swl) i8=12j 
else if (xkkmlCOl > sw2) i8=24j 
else if (xkkmlC03 > sw3) i8=36; 
else if (xkkmlCOl > sw4) i8=48; 
else iS=60; 
for ( i=0; i<4; i++) 
f 

j=3*i+i8; 
xbCi 3=hydroC j 3 ; 
ybCi 3=hydroC j + 13; 
zbCi 3=hydroC j+23 ; 

> 

> 



/* GET THE TRUE TIMES AND THE TRUE POSITIONS */ 
fscanf <fptrj,"7.1f '/.If 7.1f", 

&dataC03 ,&dataC 13 ,S<dataC23 ) ; 
fscanf (fpzic, "7.1f 7. If 7.1f 7.1f", 

&zicC03 ,&zicC13 ,&zicC23 ,&zicC33> ; 
truedC03=dataC03 ; truedC 1 3=dataC 1 3 ; 
truedC23=dataC23 ; 



/* FIRST GET HROW-CALCULATE GAIN, ESTIMATE */ 
/* COVARIANCE OF ERROR BASED ON ONE TIME */ 
/* MEASUREMENT-TC , TX , TY , TZ */ 

adapt: for (i=0; i< js; i++) 

x=xkkmlC03-xb[i3; 
y=xkkml C23-yb C i 3 ; 
z=xkkmlC43-zbCi 3; 



denom=sqrt (x*x+y*y+z*z ) ; 
auxh=vel m/denom; 
hrowC03=x*auxh ; 
hrowC23=y*auxh ; 
hrowC 43=z*auxh; 



for ( i i =0; i i <nmax ; i i ++) 
i in=i i*nmax ; 
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gnumC i i 3=pkkmlCi in3*hrowC03+ 

pkkml Ci in+23*hrowC23+pkkml Ci in+43*hrowC43 
3 

gdenom=hrowC03*gnumC03+ 
hrowC23*gnumC23+ 
hrowC43*gnumC43+r Ci 3 ; 

/* THIS IS THE FIRST GAIN COLUMN */ 

for < i i =0; i i Cnmax ; i i ++) gi C i i 3=gnumC i i 3/gdenom 

/* printf < " \nVECTOR HROW : \n"); 

printvec (hrow,5) ; 
printf ("NnVECTOR GNUM s \n">; 
printvec <gnum,5) ; 
printf ("NnDENOMINATOR is : " ) ; 

printf ("/(e \n " , gdenom) ; 
printf < "NnVECTOR GI : \n")j 
printvec (gi ,5) ; */ 

/* CALCULATE THE COVARIANCE OF ERROR PI */ 
for < i i =0; i i Cnmax ; i i +=2) 
f 

pdumCii3=(-giC03*hrowCii3) j 
pdumC ii+53=(-giC13*hrowCii3) ; 
pdumCi i+103=(-gi C23*hrowCii 3) ; 
pdumC i i+153=(-gi C33*hrowC i i 3 ) ; 
pdumCi i +203= (-gi C43*hrowC i i 3 ) ; 

> 

pdumC03+=l . 0; 
pdumC 123+=1 . 0; 
pdumC243+=l . 0; 

pi C03 =pdumC03*pkkml C03+pdumC23*pkkml C 103+ 
pdumC43*pkkml C203 ; 

pi C 1 3 =pdumC03*pkkml C13+pdumC23*pkkmlC113+ 
pdumC 43 *pkkml C21 3 ; 

pi C23 =pdumC03*pkkml C23+pdumC23*pkkml C 123+ 
pdumC43*pkkml C223 ; 

pi C33 =pdumC03*pkkml C33+pdumC23*pkkml C 133+ 
pdumC43*pkkml C233 ; 

piC43 =pdumC03*pkkmlC43+pdumC23*pkkmlC143+ 
pdumC 43 *pkkml C243 ; 
piC53=piC13; 

pi C63 =pdumC53*pkkml C13+pkkmlC63+ 

pdumC73*pkkml C 1 1 3+pdumC93*pkkml C21 3 ; 
pi C73 =pdumC53*pkkml C23+pkkmlC73+ 

pdumC73*pkkml C 123+pdumC93*pkkml C223 ; 
piCS3 =pdumC53*pkkmlC33+pkkmlC83+ 

pdumC73*pkkml C 133+pdumC93*pkkml C233 ; 
pi C93 =pdumC53*pkkml C43+pkkmlC93+ 

pdumC73*pkkml C 143+pdumC93*pkkml C243 ; 
pi C 103=pi C23 ; 
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pi C 1 1 3=pi C7D ; 

pi C 12]=pdumC 10D*pkkml C23+pdumC 12D*pkkmlC123+ 
pdumC 143*pkkml C223 ; 

pi C 13D=pdumC 10D*pkkml C3D+pdumC 12]*pkkmlC13D+ 
pdumC 143*pkkml C23D ; 

pi C 14I]=pdumC 103*pkkml C43+pdumC 123*pkkmlC14D + 
pdumC 14]*pkkml C24D ; 
piC15D=pi C3D; 
pi C16D=pi C8D ; 
pi C173=pi C13D; 

pi C 183 s pdumC 15D*pkkml C3D+pdumC 17D*pkkmlC13D + 
pkkmlC183+pdumC19D*pkkml C233 ; 
piC19D=pdumC15D*pkkml C4D+pdumC 17D*pkkmlC14D+ 
pkkmlC19D+pdumC19!]*pkkml C24D ; 
pi C20:=pi C43 ; 
pi C21 3=pi C9D ; 
piC223=piC14D; 
pi C23D=pi C19D; 

pi C24D !5 pdumC203*pkkml C4]+pdumC22D*pkkml C 14D + 
pdumC24D*pkkml C24D ; 

/* CALCULATE FIRST MEASUREMENT PREDICTION */ 
zhat=vel m*denom; 
zdiff Ci D=zicCi D-zhat; 

/* COMPUTE THE GATE FOR ERRONEOUS TIME */ 

/* MEASUREMENTS */ 

pkl s =(piCOD < 0) ? -piCO] : piCOI; 

pk3= <pi C 12D < 0) ? -pi C 12D : piC12D; 

pk5= (pi C24D < 0) ? -pi C243 : piC24D; 

pll= 3 <pkl > pk3) ? pkl : pk3; 

p = (pll > pk5) ? pll : pk5; 

/* pgate=sqrt <p) *velm; */ 

x=p+rCi D; 
if < x != 0.0) 

< 

if (x < 1.0) x = l . 0; 
do 

ol dx=x ; 

x=< x + p/x )*0.5; 
temp=ol dx-x ; 

rabs=<temp < 0) ? -temp : temp; 

> 

while (rabs > 0.0); 

> 

pgate=x*velm; 
gate=3. 0*pgate; 
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/* printf ("\nMATRIX PDUM ; \n")j 

printmat (pdum,5,5) ; 
printf ("\nMATRIX PI s \n"); 
printmat (pi ,5,5) ; 
printf ("\nZDIFF is s ")j 
printf ("\y£e \n " ,zdi f f Ci 3 ) ; 
printf ("\nGATE is : "); 
printf ("\/£e \n",gate); */ 

/* EDIT INVALID TIME MEASUREMENTS */ 
zgate=(zdiff Ci 3 < 0) ? -zdiffCi3 : zdiffCi3; 
if (zgate >= gate) 

C 

for ( i i =0; i i Cnmax ; i i++) giCii3=0.0; 
zdiff Ci 3=999. 0; 

/* TAG FOR INVALID TIME MEASUREMENT */ 

> 

/* CALCULATE ESTIMATE BASED ON ONE MEASUREMENT*/ 

/* PREDICTION */ 

for ( i i =0; i iCnmax ; i i ++) 

xiCii3=xkkmlCii3+giCii3*zdiffCi3; 

/* printf <"\n VECTOR XI : \n"); 

printvec <xi ,5) ; */ 

for ( i i =0; i i Cnmax ; i i ++) 

xkkmlCiiI=xiCii]; 

pkkmlCii3=piCiiI; 

> 

for ( j j=nmax ; j j<maxsq; j j++) pkkml C j j I=pi C j j 1 ; 
if (i == 3) goto 156; 

> /* end JS loop */ 

/* NOTE : CALLED ORIGINAL X (0/-1 ) , XKKM1 . UPDATED */ 
/* AFTER 1 MEASUREMENT CALLED IT XI, THEN MADE */ 
/* XKKM1 AND WENT THRU ITERATION AGAIN. AFTER */ 
/* YOU HAVE UPDATED XKKM1 FOR EACH MEASUREMENT */ 
/* XKK=XI AND PKK=PI. */ 

156: for (i i =0; i i Cnmax ; i i ++) 

C 

xkkCi i 3=xi Cii 3; 
pkkCi i 3=pi Ci i 3 ; 

> 

for ( j j=nmax ; j j<maxsq; j j++) 

pkkC j j3=pi C j j3; 

> 
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* 



/* print-f ( "\nUPDATING VECTORS INTERVAL ... \n") 

printf ('' \nVECTOR XKKM1 : \n"); 
printvec (xkkml ,5) ; 
printf ("\nMATRIX PKKM1 : \n">; 
printmat (pkkml ,5,5) ; 

print-f ("NnVECTOR XKK : \n"); 
printvec (xkk,5) ; 
print-f ("XnMATRIX PKK : \n">; 
printmat (pkk, 5,5) ; */ 

/* RECALCULATE TIME MEASUREMENTS, and FORM */ 

/* ABSOLUTE VALUE OF RESIDUALS. */ 

-for <i=0; i<4; i++) 

/* EDIT INVALID TIMES FOR ADAPTIVE MANEUVER */ 
/* ROUTINE */ 

{ 

if (zdiffCi] < 999.0) 

x=xkkmlCO]-xbCi I; 
y=xkkml C2]-ybCi 3 ; 
z=xkkmlC4D-zbCi ]; 

/* denom=sqrt (x*x+y*y+z*z ) ; */ 
denom=x*x+y*y+z*z ; 
x=denom; 
if (x != 0.0) 

if (x < 1.0) x = 1 . 0 } 
do 

oldx=x ; 

x=( x + denom/x )*0.5; 
temp=ol dx-x ; 

rabs=(temp < 0) ? -temp : temp; 

> 

while (rabs >= l.e-12); 
denom=x ; 

zhat=vel m*denom; 
temp=zicCi 1-zhat; 

zdi f f C i D= ( temp < 0) ? -temp : temp; 

> 

el se 
C 

zdiffCi 1=0. 0; 
nzdif f-=l ; 

> 

y 
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/* THIS FUNCTION COMPUTES THE ADAPTIVE Q MATRIX */ 
i-f <i curve == 1) 

bb=xkkml C33*xkkmlC33; 
dd=xkkml C 1 3*xkkml C 1 3 ; 
al=bb+dd? 

bd=xkkml C13*xkkmlC33; 

el=dd*sigacc/al+bb*sigcc; 

el2=bd*si gacc/al-bd*si gcc; 

e2=bb*si gacc/a 1 +dd*si gcc ; 

qC03=g2*el ; 

qC 1 3=g3*el ; 

qC23=g2*el2; 

qC33=g3*el2; 

qC63=a2*el ; 

qC73=qC33 ; 

qC83=a2*el2; 

qC 123=g2*e2; 

q[ 133=g3*e2; 

qC 183=a2*e2; 

> 

el se 

{ 

bb=gammaC03*gammaC03 ; 
dd=gammaC 1 3*gammaC 1 3 ; 
qC03=bb*covwC03 ; 

qC 1 3=gammaC03*covwC03*gammaC 1 3 ; 
qC23=0.0; qC33=0.0; 
qC63=dd*covwC03 ; 
qC73=0. 0; qC83=0.0; 
qC 123=bb*covwC 1 3 ; 

qC 133=gammaC03*covwC 1 3*gammaC 1 3 ; 
qC 183=dd*covwC 13 ; 

> 

qC53=qC13; qC103=qC23; qC113=qC73; 

qC153=qC33; qC 163=qC83 ; qC 173=qC 133 ; 

/* print-f ("NnMATRIX Q : \n"); 

printmat (q,5,5) ; */ 

/* IF ALL TIME MEASUREMENTS EXCEED GATE, BYPASS */ 
/* ADAPTIVE MANEUVER ROUTINE. */ 

i-f (nzdi-f-f != 0.0) 



zdi-f av= (zdi-f-f C03+zdi-f-f 1113+zdi-f-f C23+zdi-f-f C33 ) /nzdi -f -f ; 
/* print-f ("\nZDIFAV is : " > ; 

print-f <"\%e \n" , zdi-f av) ; */ 
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/* DO NOT PERFORM ADAPTIVE MANEUVER IF FILTER */ 
/* HAS NOT ACHIEVED THE STEADY STATE. */ 

i-f <kk > 3) 

< 

/* dost i me ( &i hours , &i mi ns , 8<i secs , &i ths ) ; 

hours=ihours; mins=imins; secs=isecs; 
ths=i ths; 

time=3600. *hours+60. *mi ns+secs+O. 01*ths; 
printf("%s V.e " timel = " , time); */ 

/* IF ZDIFAV MEETS CRITERIA TRANSFER OUT */ 
/* OF ADAPTIVE MANEUVER ROUTINE. */ 

i-f (zdi-fav > value && icont < maxopt) 

i 

/* INCREASE BAIN */ 

-for ( j=0; jCmaxsq; j++) 

pkkmlCj3=pkkCj]+qCj3; 

/* print-f ("\n INCREASE GAIN AND PERFORM"); 
printfC ADAPTIVE ... \n">; 
print-f ("\nMATRIX PKKM1 : \n"); 
printmat (pkkml ,5,5) ; */ 

/* PERFORM ADAPTIVE MANEUVER BY */ 

/* REITERATING SAME TIME SLOT. */ 
i cont+=l ; 
goto adapt; 

> 

> 

> 

icont=0; 
nzdi f -f =4; 

xdi f -f COD=x kk CO] -trued CO] ; 
xdi f f C 1 3=xkkC 23 -trued C 1 3 ; 
xdiff C23=xkkC43-truedE23; 

/* temp=xdi ffE13*xdiffC13/pkkmlC123; 

i-f (temp < limvar) icurve=0; 
else icurve=l; */ 

fprintf (-fpe, "\n V.d 7.e 7.e 7.e",kk, 

xdi -f-f C03, xdiff E 1 □ , xdi -f-f C23) ; 

/* WRITE */ 

/* print-f ("\nVECTOR ERROR : XDIFF YDIFF ZDIFFXn" ) ; 

pri ntvec (xdi f f , 3) ; */ 

/* PREDICTIONS FOR PKKM1 */ 

for ( i=0; i <nmax ; i ++) 
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/* 



/* 



/* 



< 

phipkkCi D=pkkCi 1+pkk Ci +53*phi sCOD ; 
phipkkCi+51=pkkCi+5] ; 

phipkkCi+10]=pkkCi+10]+pkkCi+151*phisC21; 
phipkkCi+15J=pkkCi+15J ; 
phipkkCi+20D=pkkCi+201; 

> 



■for (i=0; i<nmax ; i++) 

C 

in=i*nmax ; 

pktempCin]=phipkkCin3+phipkkCin+l]*phisCOD; 
pktempCin+1 D=phipkkCin+l ] ; 

pktempCin+2]=phipkkCin+21+phipkkCin+3D*phisC2] 
pktempCin+3]=phipkkCin+31 ; 
pktempCin+4J=phipkkCin+41 ; 

> 

for ( j=0; j<maxsq; j++) pkkml C j l=pktempC j 1+qC j 3 ; 

printf ("\nPREDICTIONS FOR PKKM1 ... \n")j 
printf ("NnliATRIX PKKIil : \n"); 
printmat (pkkml ,5,5) ; */ 

/* CALCULATE NEW XKKM1 */ 
for ( i i=0; i i <nmax ; i i ++) 
f 

x kkml CiiD=xkkCiil; 

/* pl=sqrt (f abs (pkkCi i 1 ) ) ; */ 

> 

xkkml COD=xkkml C01+phisC01*xkkC 1 1 ; 
xkkmlC21=xkkml C23+phi sC2]*xkkE33 ; 



printf ("NnNEW XKKM1 ... (for next time"); 
printf (" interval ) \n") ; 
printf ("\nVECTOR XKKM1 : \n"); 
printvec (xkkml , 5) ; */ 

f printf (fpp,"\n 7.d 7.e 7.e 7.e",kk, 
pkkC01,pkkC12D,pkkC241) ; 

f printf (fpxyz , "\n V.d Y.e V.s 7e",kk, 
xkkCOl ,xkkC21 ,xkkC41) ; 

/* PLOTTING */ 
col =xkkC01*x seal e+bx; 
raw=xkk C21*yscal e+by; 
bioswd (row, col ) ; 



bi osset (3,0) ; 

printf ("7.s 7.. lf","x =",xkkC01) 
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9 



bi osset (3,13) ; 

print-f ("7.s 7.. =" ,xkkC2J ) ; 

bi osset (3,25) ; 

print-f ("7.s 7.. 1-f" , "z =",xkkC4]); 
bi osset (3,42) ; 

print-f ("7.s 7. 1-f ", "vx =",xkkC13>; 
bi osset (3,54) ; 

print-f ("7.s 7.. 1-f " , " vy =" ,xkkC3U ) ; 
biosset (3,65) ; 

print-f ("7.s 7.d","time =" ,kk); */ 

> /* end TIME loop */ 

-f cl ose (-fpxyz ) ; 

-f cl ose (-fpe) ; 
f cl ose (-fpp) ; 

-fclose (-fpzic) ; 
f cl ose (-fptr j ) ; 
c=keyrd ( ) ; 
biosini (3) ; 

quit: 

print-f ("\nquit ? CY/N] :"); 
scan-f ("7.s" ,&c) ; 

i-f (c== 'n ' i! c== ' N ' ) goto restart; 

> /* end MAIN PROGRAM */ 
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/******************************************■* •***■* ■*■*■***■•**•*•*•*•*/ 



/* */ 

/* PRINTVEC will print the vector elements given */ 

/* the name and the dimension. */ 

/* */ 

/* PRINTMAT will print the matrix elements in the */ 

/* usual matrix form, given the dimensions. •*/ 

/* */ 

/* */ 



/**********************************************************/ 

/* 

printvec (vector , dim) 
int dim; 

double vectorCl; 

C 

int i, j; 

for ( j=0; j<dim; j++) printf ( "%e %ls" , vectorCj 3 , " "); 
printf <"\n"> | 

} 

printmat (matrix , row, col ) 
int row, col; 
double matrix Cl; 

C 

int i,j; 

for (i =0; i Crow; i ++) 

i 

for ( j=0; j<col ; j++) printf ( '"/ie 
%ls" ,matrixCi*col+jl , " "); 
printf ("\n") ; 

> 

> */ 



167 



/***********************************************•*■*•*****•**•*•*/ 



/* */ 

/* FRAME */ 

/* Draw a frame on the screen. */ 

/* ■*•** NOTE *** */ 

/* The upper left corner of the screen is 0,0 and the */ 

/* bottom right corner is 199,639. */ 

/* */ 

/* */ 



/***••*** *************************************************** •*•*■*■*/ 

int frame (ul row, ul col , 1 rrow, lrcol ) 
int ul row ,ul col , 1 rrow, 1 rcol ; 

{ 

i nt count , i r ow , i , i aux , i col , tcount ; 

/* do top line first */ 

for <icol = ulcol; icol <= lrcol; icol++> 
bioswd ( ul row, icol ) ; 

/* do both sides at once */ 

for' (irow = ulrow+1; irow < lrrow; irow++) 

bioswd (irow, ulcol ) ; 
bioswd (irow, lrcol ) ; 

> 

/* do bottom line */ 



for (icol = ulcol; icol < = lrcol; icol++> 

bioswd (lrrow,icol ) ; 

/* */ 



/* do vertical center line with scale ■*/ 
i =0. 5* (ul col +1 rcol ) +0. 5; 

for (irow = ulrow+1; irow < lrrow; irow++) 
for (irow = ulrow+16; irow < lrrow; irow+= 
for ( tcount=i -4; tcount <= 

bioswd (irow, tcount) ; 

/* do horizontal center line with scale */ 
i =0. 5* (ul row+lrrow) +0. 5; 

for (icol = ulcol+1; icol < lrcol; icol++) bi oswd ( i , i col ) ; 
for (icol = ulcol+60; icol < lrcol; icol+=60> 

for (tcount=i-3; tcount <= i+3; tcount++> 

bioswd (tcount, icol ) ; 

/* */ 

return (0) ; /* done •*/ 

> 



bioswd ( i row , i ) ; 
16) 

i+4; tcount++) 
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/**#■*•*•*■*•*•* *************************************************/ 



/***************#***********#*************#***■*■*•*•**■*•#■*##*■*•*/ 

#include "stdio.h" 
ttinclude "math.h" 

#include "kalman.h" 

tra jec (narray, hydro, xb , yb , zb ,xkkml , jstart , jend ,wd ,g ,worg) 
double hydro C 3,xbC3,ybC3,zbC3,xkkmlC3,wd,g; 
int narray ,worg , jstart , jend; 

C 

int pli , i , start , end; 

double pp 1 ,r 1 , sqrt ( ) ,zcs,zicC43 ,zi C43 ; 
double xd , yd , zd , v ,den , velm , gg ; 

static double xn E63 = -C-3. 01 , -2.0, -1.5, 1.0, -0.6, 

0.03; 

static double ynC63=CO.O, 0.0228, 0.0668, 0.1357, 

0.2743, 0. 53 ; 

static double snC53 = -C43. 8596, 11.3636, 7.25689, 

2.891352, 2. 658873; 

double dataC53,wr; 
int runn,i8; 

FILE *fptrj, *fopen(); 

FILE *fpzic, *fopen(); 
int j,jj; 

/* THIS ROUTINE COMPUTES TRUE TRAJECTORY OF TORPEDO 
dataC03=true x position, dataC13= true y position 
dataC23=true z position, w=turn rate 
dataC33=x_vel oci ty , dataC43=y_vel oci ty 
jstart=time slot where maneuver starts 

jend=time slot where maneuver ends */ 

dataC03=xkkmlC03; dataC 1 3=x kkml C23 ; dataC23=x kkml C43 ; 
dataC33=x kkml E 1 3 ; dataC43-x kkml C33 ; velm=l . 0/vel ; 
r 1=0. 49; 

wr=twopi*wd/360. 0; gg=32.2*g; start= jstart ; end=jend; 

printf("\n 7.s \n", "start execution of trj.c"); 
f ptr j=f open ( " tr j . dat " , "w" ) ; 
f pzic=f open ( "zic. dat " , "w" ) ; 



/* 

/* 

/* 



TRAJECTORY SIMULATION ROUTINE - file: TR. C 



*/ 

*/ 

*/ 



for ( j j=0; j j<npt; j j++) 



C 

/* GET HYDROPHONE ARRAY COORDINATES */ 
if (narray != 0) 
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•r 



if (dataC03 > swO) 


i 8=0; 




el se 


if 


<dataC03 


> 


swl ) 


i 8=12 


el se 


if 


(dataC03 


> 


sw2) 


i8=24 


el se 


if 


(dataC03 


> 


sw3) 


i 8=36 


else 


if 


(dataC03 


> 


sw4) 


i 8=48 


el se 


i 8= 


60; 








for 


<i=0 


; i<4; i++) 









j=3*i +i 8; 
xbC i 3=hydroC j 3 ; 
ybCi 3=hydroC j+13; 
zbCi 3=hydroC j+23 ; 
> 



TIMES AND THE TRUE POSITIONS 
TRUE POSITIONS */ 



/* COMPUTE THE TRUE 
WRITE TRUE TIME and 
printf ("7.s" , 

fprintf (fptr j , "\n 7.e 7.e Xe" , 

data! 03 , data! 1 3 , dataC23 ) ; 

for <i=0; i<4; i++) 

{ 

xd=dataC03-xb L i 3; 
yd=dataC 1 3-ybCi I; 
zd=dataC23-zb C i 3; 
den=sqrt (xd*xd+yd*yd+zd*zd) ; 
z i C i 3 =vel m*den ; 



/* 



/* 



TIMES 



printf ( "\nTRUE 
printvec (zi ,4) ; 
printf <"\nTRUE POSITIONS 
printvec (data, 3) ; */ 



\n" ) 



\n " ) 



/* SET NOISE, SCALE AND ADD TO TRUE */ 
/* MEASUREMENT TIME */ 
ppl=3I7. 0*rl ; 
pli=ppl ; 
r l=pp 1-p ii ; 
v=r 1 ; 
j=0; 
if (v 
if (v 
j+=l; 
goto r2; 

r8: v= ( v-yn C j 3 ) *sn C j ]+xn C j 3 ; 
if (rl >= 0.5) v=<-v) ; 
zcs= ( v/3. 00) *0. 00001 ; 

printf ("\nRANDOM NUMBER : " ) ; 
printf ("Xe \n",zcs);*/ 



r2: 



> 0.5) v=l . 0-r 1 ; 

< ynCj+13) goto r8; 
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9 



zicCi ]=zcs+zi Ci 3 ; 

> 

f print f (f pzic, "\n 7.e 7.e 7.e 7.e" , 

zicC03,zicC13,zicC23,zicC33) 

/* NEW DATA */ 
if (start >= npt) line (jj ,0,npt , data) ; 
el se 

i 

if (end > npt) end=npt; 

line( jj,0, start , data) ; 

circl e ( j j , start , end , data, wr , gg , worg ) ; 

1 ine ( j j , end , npt , data) ; 

> 

> 

f close (f ptr j ) ; 
f close (f pzic) ; 



circl e (j , start ,end , data, ww,gg , worg) 
double dataC3 ,ww,gg; 
int j , start , end , worg; 
i 

double sin ( ) , cos ( ) ,atan ( ) ; 

doubl e r , vt , aux , a , b , teta, tetaO ,m , al -f a , wa , ga , g , w 
g=gg; w=ww; 

i -f ( j >= start 8 <& j < end) 

•C 

i-f (dataC33==0. 0 && dataC43==0. 0) goto out; 
vt=sqrt (dataC33*dataC33+dataC43*dataC43 ) ; 
if ( worg== 1 ) g=w*vt; 
else w=g/vt; 
r=vt/w; 

if <dataC33==0. 0) 

C 

b=dataC 1 3 ; 
if (dataC43 >0. 0) 

a=dataC03-r ; 
teta0=0. 0; 

> 

el se 

a=dataC03+r ; 
teta0=0. 5*twopi ; 

> 

> 

else if (dataC43==0. 0) 
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£ 

a=dataC03 $ 
if (dataC33 >0. 0) 

£ 

b=dataC 1 3+r ; 
teta0=-0. 25*twopi ; 

> 

el se 
£ 

b=dataC 1 3-r ; 
teta0=0. 25*twopi ; 

> 

> 

el se 
£ 

m=dataC43 /dataC33 ; 
aux=r/sqrt ( 1 . 0+m*m) ; 
i-f (dataC33 > 0.0) aux=-aux; 
b=dataC 1 3-aux ; 
a=dataC03+m* (dataC 1 3-b) ; 
tetaO=atan2(-dataC33,dataC43) ; 

> 

teta=l . 31*w+teta0; 
dataC03=r*cos (teta) +a; 
dataC 1 3=r*si n (teta) +b ; 
dataC33=-vt*sin (teta) ; 
dataC43=vt*cos (teta) ; 
i-f (j==end-l) 

£ 

wa=w*360. 0/twopi ; 
ga=g/32. 2; 

pri nt-f ( " \n ... from TRAJEC : "); 

print-f ( "\n" ) ; 

pri nt-f (" \n%s V.e V. s"," turn rate : ",wa, 

" dg/seg " ) ; 

print-f ( "\n/is %.2f\n" g— number : ",ga) 

> 

out : 

> 

> 

1 ine ( j , start , end , data) 
double dataC 3; 
int j, start, end; 

£ 

if((j >= start) && (j < end)) 

£ 

dataC03+=1.31*dataC33; 
dataC 13+=1.31*dataC 43; 

> 
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APPENDIX F 



LISTING OF CIEKR1.C - CIE VERSION 

/***•*•»•»•»*#•*•*•»•*•»•»•»•»***•*■»■*■»■»■»********■)(.■»■»■»■»■»■»■»**■»****■*■»■***•»*•»*/ 
/* */ 

/* HEADER FILE - KALMAN. H */ 

/* CIE MICROCOMPUTER VERSION */ 

/* */ 

, *********************************************************** 



#def ine 


nmax 


5 


/* 


# o-f col. in pkk, q ... */ 




#def ine 


maxsq 


25 


/* 


size o-f equiv. vector PKK, 


Q. . . */ 


#def ine 


js 4 




/* 


# hydroph. in any hydrophone */ 








/* 


array */ 




#de-f ine 


hcmax 


12 


/* 


# of col. in matrix HYDRO 


of */ 








/* 


hydroph. coord. */ 




#def ine 


hmax 


72 


/* 


equiv. vector size of HYDRO */ 


#def i ne 


dmax 


3 








#de-f ine 


i time 


150 


/* 


max.# of time slots */ 




#def ine 


npt 


150 








#d e-fine 


i cmax 


5 


/* 


max.# of optimal iterations */ 








/* 


allowed ■*/ 




#d e-fine 


cri ter 0. 1 


/* 


criteria to compare ZDIFAV 


*/ 


#def i ne 


na 0 




/* 


may assume ONLY values 0 or 6 */ 








/* 


0 =>single;6=> multiple arrays */ 


#de-f ine 


xpos 


10000. 0 


/* 


initial torpedo x-coordinate */ 


#de-f ine 


x vel 


-50.0 


/* 


initial torpedo x-velocity 


*/ 


#def i ne 


ypos 


0.0 


/* 


initial torpedo y-position 


*/ 


#de-f i ne 


yvel 


5. 0 


/■* 


initial torpedo y-velocity 


*/ 


#def ine 


zpos 


0.0 


/* 


initial torpedo z-position 


*/ 


#def i ne 


swO 


33000.0 


/* 


swi define points where 


*/ 


#def ine 


swl 


27000. 0 


/* 


switching from one to 


*/ 


#de-f ine 


sw2 


21000.0 


/* 


another 


*/ 


#def ine 


sw3 


15000. 0 


/* 


array 


*/ 


#de-f i ne 


sw4 


9000. 0 


/* 


is done. 


*/ 


#de-f ine 


sigdiv 1.0 








#de-f ine 


see 0 


. 387463094 


/* 


- in radians. . . 


*/ 


#de-f ine 


sacc 


36.2 








#def ine 


vel 4860.0 


/* 


speed of sound 


*/ 


#def i ne 


twopi 


6.28315308 






#def i ne 


rrvar 


1 . e-5 








#def ine 


rvar 


l.e-10 








#de-f ine 


pkkdiag 1000.0 








#def ine 


1 i mvar 1 . 5 




/* defines threshold to 


use Q*/ 








/* 


constant */ 




#de-f ine 


gmO 0 


.86 








#de-f i ne 


gml 1 


.31 








#de-f i ne 


cwO 1 


.0 


/* 


initial was 1036.84 */ 




#de-f ine 


cwl 1 


.0 


/* 


initial was 1036.84 */ 




#de-f i ne 


cw2 1 


.0 
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/**************■*■*•#•#■*•* ##*#-*•»*•»•»-»***# **##**####*######**#*#*#/ 



(♦include "stdio.h" 

#include "math.h" 

#include "kalman.h" 
double 

xkkml Cnmax 3 , pkkml Cmaxsq] , pkk Cmaxsq] ,xkk Cnmax ] , dataCdmax 1 ; 

/* STARTING THE MAIN PROGRAM... */ 

main ( ) 

{ 

FILE *fpxyz, *fopen(); 

FILE *-fpe, *-fopen O ; 

FILE *-fpp, *-fopen<); 

FILE *fptrj, *-fopen(); 

FILE *-fpzic, *-fopen(); 

FILE *-fparam, *-fopen(); 

FILE *-fpin, *-fopen(); 

int i hours, imins, isecs, i ths,col ,row; 

int i 8 , i , j , kk , i in , i cont , i curve , opti on , par am; 

int tr jopt , nrange; 

int ii,jj,narray,nzdi-f-f,in,pli, runn , c ; 
int j start , j end , worg ; 

■float rabs,zgate; 

double hei ght , wi dth , ymax , ymi n , xmax , xmi n; 
double xscal e, yscal e , bx , by ; 

double hours , mi ns , secs , ths , ti me , ol dt i me , del tat ; 

double sqrt () ,oldx , value; 

double denom,al4,gdenom,gate,zdi-fav; 

double den , velm, temp ; 

double a2 , g 1 , g2, g3 , pi , xdi -f -f C31 ; 

double zhat,zcs,dk; 

double v, truedC3] ,wd ,g; 

double x , y , z , auxh , xd , yd ,zd , si gcc , si gacc; 

double al , el , el2 , e2 , bb , dd ,bd; 

double pkl,pk3,pk5, p gat e, r gate, p , p 1 1 ; 

double phi pkk Cmaxsq 1 , pk temp Umax sql , qCmaxsq] ; 

doubl e pi Cmaxsq 1 , pdum Cmaxsq 1 , qtemp Cmaxsq 3 ; 

double gnum Cnmax 1 , gi Cnmax 3 , x i Cnmax 3 , hrow Cnmax 3 ; 

double zicCjsl,ziCjs],zdi-ffCjsl,rCjsl,phisCjsl,ztCjsl; 

double xbCjs],ybCjsD,zbCjsl,rrCjsl; 

double hydroChmax 1 , covwC31 , gamma C21 , opt C5I ; 

•f pxyz=-f open ( "xkk. dat" , "w" ) ; 

■f pe=f open ( "xdi f f . dat " , "w" ) ; 
fpp=f open ( "pkk . dat " , 11 w" ) ; 



/* 

/* 

/* 

/* 



KALMAN FILTER PROGRAM FOR THE CIE MICRO 
(FILE CIEKR1.C) 



*/ 

*/ 

*/ 

*/ 
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*/ 



/* INITIALIZE CONSTANTS, HYDROPHONE MATRIX, PHI, R, 
DATA FOR TRAJECTORY, AND LOAD X(0/-1), P(0/-1) 

nzdiff=4; icont=0; icurve=l; value=cri ter; wd=5.730; 
runn=l; narray=na; param=0; worg=l; g=0. 156; 
nrange=0; 

j start =40; jend=80; 



hydroC03=36000. 0; 
hydroC33=36030. 0; 
hydroC63=36000. 0; 
hydroC93=36000. 0; 

hydroC 123=30000. 0; 
hydroC 153=30030. 0; 
hydroC 183=30000. 0; 
hydroC21 3=30000. 0; 

hydraC243=24000. 0; 
hydro [273=24030. 0; 
hydroC303=24000. 0; 
hydroC333=24000. 0; 

hydroC 363 =18000. 0; 
hydro C39 3 =18030. 0; 
hydroC423 = 18000. 0; 
hydroC453=l8000. 0; 

hydroC483 = 12000. 0; 
hydro C51 3 = 12030. 0; 
hydroC543=12000. 0; 
hydro C57 3 = 12000. 0; 

hydroC603=6000. 0; 
hydroC633=6030. 0; 
hydroC663=6000. 0; 
hydroC693=6000. 0; 



hydroC 13= 6000.0 
hydroC43= 6000.0 
hydroC73= 6030.0 
hydroC 103=6000. 0 

hydroC 133=6000. 0 
hydroC 163=6000.0 
hydroC 193=6030. 0 
hydro [223=6000. 0 

hydroC253=6000. 0 
hydroC283=6000. 0 
hydroC31 3=6030. 0 
hydroC343=6000. 0 

hydroC373=6000. 0 
hydroC403=6000. 0 
hydroC433=6030. 0 
hydro [463=6000. 0 

hydroC493=6000. 0 
hydro [523=6000. 0 
hydroC553=6030. 0 
hydroC583=6000. 0 

hydroC61 3=6000. 0 
hydro [643=6000. 0 
hydroC 67 3 =6030. 0 
hydroC703=6000. 0 



hydroC23= 0.0; 
hydroC53= 0.0; 
hydroC83= 0.0; 
hydroC113= 30.0; 

hydroC143= 0.0; 
hydroC173= 0.0; 
hydroC203= 0.0; 
hydroC233= 30.0; 

hydroC263= 0.0; 
hydroC293= 0.0; 
hydroC323= 0.0; 
hydroC353= 30.0; 

hydroC383= 0.0; 
hydroC41 3= 0.0; 
hydroC443= 0.0; 
hydroC473= 30.0; 

hydroC503= 0.0; 
hydroC533= 0.0; 
hydroC563= 0.0; 
hydroC593= 30.0; 

hydroC623= 0.0; 
hydroC653= 0.0; 
hydroC683= 0.0; 
hydroC71 3= 30.0; 



r CO 3 =rvar; rC13=rvar; rC23=rvar; rC33=rvar; 

gammaC03=gm0; gamma C 1 3=gml ; 
covwC03=cw0; covwC13=cwl; covwC23=cw2; 

x kkml [03 = 1000. 0; x kkml Cl 3= (— 50. 0) ; xkkml C23=0. 0; 
x kkml C33=5. 0; xkkml C43=0. 0; 

for <i =0; i <nmax ; i ++) 

for ( j=0; j<nmax ; j++) pkkml Ci *nmax+ j 3=0. 0; 
for <i=0; iCnmax; i++) pkkml Ci *nmax+i 3=pkkdi ag ; 



phi sC03=l . 31 ; phisC13=0.0; phi sC23=l . 31 ; phisC33=0.0; 
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) 



xbC03=<-15.0) 
xbC13= 15.0; 
xbC2D= (-15. 0) ; 
xbC33= (-15. 0) ; 



yb C03= (-15. 0) ; 
ybC 1 3= (-15. 0) ; 
ybC23= 15.0; 
ybC33= (-15. 0) ; 



zbC03= (-15. 0) ; 
zbC 1 3= (-15. 0) ; 
zbC23= (-15. 0) ; 
zbC33= 15.0; 



/* GET DATA INPUT */ 



/*#***********************■*■*■**■■#■*■**■*■*■*■*■*■**■*#■*•*■*■*■*•*■*■******■*■#■*/ 
/* */ 

/* DATA INPUT */ 

/* */ 

/****•*•**■**■*****•***■#■*■#*****■*•*■*■*■#■■#****•*•**■#■■*■**•*■*•*•*•*•**■#■■#■*■#***■*•*/ 



menu: /* OPTIONS MENU */ 
print-f < "\n" ) ; 

printf <"\n KALMAN FILTER ">; 

print-f ( " \n " ) ; 



pri ntf < 


" \n 




OPTIONS :"); 


printf ( 


"\n" ) ; 






pri ntf ( 


" \n 


1) 


RUN TRAJECTORY"); 


printf ( 


"\n" ) ; 






pri ntf ( 


" \n 


2) 


RUN KALMAN"); 


printf ( 


" \n " ) ; 






pri ntf ( 


" \n 


3) 


MODIFY PARAMETERS"); 


printf ( 


" \n " ) ; 






printf ( 


" \n 


4) 


QUIT") ; 


printf ( 


" \n " ) ; 






if (param!=0) 







{ 

print-f ( "\n" ) ; 

print-f ("\nA parameter was changed ... " 

print-f (" You may have to run"); 
print-f (" TRAJECTORY before continue."); 
> 



printf ( " NnEnter value (1-4) : "); 

scanf ( " V.d " , ?<opti on ) ; 
if (option =as l) 

C 

f pin=f open < "tr j . inp" , "w" ) ; 
f printf (f pin, "V.d V.d" ,narray , runn ) ; 
f printf <f pin, "\n'/.e 7.e 7.e", 

xkkmlC03,xkkmlC13,xkkmlC23) ; 
f printf (f pin, "\n7.e V.e" , x kkml C33 , x kkml C41 ) ; 
f cl ose (f pin) ; 



tra jec ( narr ay , hydro ,xb,yb,zb,xkkml , j start , jend , wd , g , worg ) ; 
param=0; 
goto menu; 

> 
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else if (option==2) 
else i-f (option==3) 
else i-f (option==4) 
else goto menu; 



goto run; 
goto modi-fy; 
goto quit; 



modi -fy: 

print-f 
printf 
pri nt-f 
print-f 
print-f 
print-f 
print-f 
print-f 
print-f 
print-f 
print-f 
print-f 
print-f 
print-f 
print-f 



( "\n 
("\n") ; 

( "\n 1) 

( "cri teria" ) 



( "\n 
( "\n 
( "\n 
( "\n 
("R") ; 

( "\n 

(" PKK M ); 
( "\n" ) ; 

( "\n 
( "\n") ; 

( "\nEnter 



2 ) 

3) 

4) 

5) 



MODIFY PARAMETERS OPTIONS :"); 

ZDIFAV stop optimization"); 

SINGLE / MULTIPLE ARRAY"); 

INITIAL X, Y, and Z positions"); 
INITIAL Vx and Vy velocities"); 
NOISE COVARIANCE - diag_elem o-f " ) 



scan-f ( "%d" , &param) ; 



6) ERROR COVARIANCE - diag_elem o-f") 

7) MAIN MENU - OPTIONS"); 

value (1-7) : "); 



switch (param) 

t 

case 1: 

print-f ("\n 7.s 7.e" , 

"Current CRITERIA is value); 
print-f ( "\nchange ? CY/N3 :"); 
scan-f ("7.s" ,&c) ; 
i-f (c== 'n ’ i! c== ' N ' ) break; 
print-f ( "\nEnter new value (real) : "); 

scan-f ("7.1 -f " Rvalue) ; 
break; 



case 2: 

pri nt-f ( "NnSINGLE array is the default"); 
if (narray==0) 

printf ("\n7.s" , "Current case is : SINGLE"); 
el se 

printf ("\n7.s" , "Current case is s MULTIPLE"); 
printf (" \nchange ? CY/NI :"); 
scanf ( "Xs" , &c ) ; 
if (c== 'n ' I I c== ' N ' ) break; 
printf ("\nEnter S for SINGLE or"); 
printf (" M for MULTIPLE array : "); 

scanf ("7.s" ,&c) ; 

if (c=='s' ! ! c=='S' ) narray=0; 

else if (c== ' m ' ! ! c== ' M ' ) narray=l; 

else narray=0; 
break ; 
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case 3: 

printf ( "\nCurrent values are : "); 

print-f ( " \nXs 7.e 7. s 7.e 7. s 7.e" , "xpos * " ,xkkmlCOD 
" ypos = ",xkkmlC2D," zpos = ",xkkmlC4D> 
printf < "\nchange ? CY/ND : " ) ; 
scarvf ("%•",&<=) ; 
i-f <c== ' n ' !! c== ' N ' ) break; 

pri ntf ( " \nEnter new values : "); 

print-f < "\n%s" , "xpos = " ) ; 
scan-f <"•/. If" ,&xkkmlCOD) ; 

print-f ( "\n‘/.s" , "ypos = "); 
scan-f <"*/.lf ",&xkkmlC2D) ; 

printf <"\n%s" , "zpos = ">; 
scanf ("‘/.If ",&xkkmlC4D> ; 

break; 

case 4: 

printf ( "\nCurrent values are : 

printf ("\n7.s 7.e ’/.s Xe","xvel = ",xkkmlClD, 

" yvel = " ,xkkmlC3D) ; 
printf ( "\nchange ? CY/ND :"); 
scanf ( "7.s" , 8<c ) ; 
if (c== ' n ' ! ! c== ' N ' ) break; 

printf ( "\nEnter new values : "); 

printf ("\n%s" , "xvel « " ) ; 
scanf ("‘/.If ",&xkkmlClD> ; 

printf ("\n%s" , "yvel = "); 
scanf ("7.1f ",&xkkmlC3D) ; 

break; 

case 5: 

pri ntf ( "\nCurrent values are : "); 

printf ("\n’/.s ‘/.e ■/. s 7.e 7. s ‘/.e 7.s 7.e M , M r COD = ", 
rCOD," rCID = M ,rCl]," rC2D = ",rC2D, 

" r C3D = " ,rC3D ) ; 
pri ntf ( " \nchange ? CY/ND :"); 
scanf < M, /.s" ,&c) ; 
if (c== ' n ' ! ! c== ' N ' ) break; 

pri ntf ( " \nEnter new values : "); 

printf ("\n'/.s" , "r COD = "); 
scanf ("■/.If" ,&r COD ) ; 

printf ("\n'/.s" , "rC ID = "); 
scanf ("’/.If " ,8<rClD> ; 

printf ("\n‘/.s" , "rC2D = "); 
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scarvf ( "7.1f " ,&rC21) ; 

printf <"\n*/.s" , "rC31 = "); 
scanf ("7.1f " ,&rC31) ; 

break ; 

case 6: 

printf < "\nCurrent values are : "); 

printf ("\n7.s 7.e V.s V.e V.s 7.e" , "pkkmlCOl = ", 
pkkml CO] , " pkkmlC61 = ", pkkml C63 , 

" pkkml C 121 = " ,pkkmlC123) ; 
printf ("\nXs V.e V.s V.e" , "pkkml C 181 = ", pkkml C 181 , 
" pkkml E241 = ", pkkml C241 ) ; 
pri ntf ( " \nchange ? CY/N1 :"); 
scanf ( "V.s" ,&c> ; 
if (c== ' n ' ! ! c== ' N ' ) break; 

printf (" \nEnter new values : 
printf ("\n7.s" ,"pkkmlC01 = " ) ; 
scanf ("7.1f",&pkkmlC0D; 

printf ("\n7.s" , "pkkml C61 = ">; 
scanf <"*/.lf " ,&pkkmlC61) ; 

printf ( "\n%s" , " pkkml C123 = "); 
scanf ("7.1f " , fcpkkml C 121 ) ; 

printf <"\n*/.s" ,"pkkmlC181 = "); 
scanf ("7.1f " ,8<pkkmlC 181) ; 

printf ("\n7.s" , "pkkml C241 = "); 
scanf ("7.1f " ,&pkkml C241 ) ; 

break; 

case 7: 

goto menu; 
break ; 

default : 
break ; 

> 

goto modify; 

run: /* START EXECUTION */ 

printf ("\n") ; 
printf ("\nEnter run # : 
scanf ( "%d " , &runn) ; 

f par am=f open ( "par am. dat " , "w" ) ; 
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/* f printf (fpxyz , "72d" ,runn) ; 
fprintf (fpe, "72d" ,runn) ; 
f printf (fpp , "72d" ,runn) ; 

■fprint-f (fparam, "72d" , runn ) ; 

if (narray==0) 

f print f (f pxyz , "7s" , " Single" ) ; 
f printf <f pe, "7s" , " Single" ) ; 
f printf (fpp , '"/.s" , " Single") ; 
f printf (f par am, "7s" , " Single") ; 

> 

el se 
£ 

f printf (f pxyz , "V.s" , " Mult") ; 
f printf (fpe, "V.s" , " Mult"); 
f printf (fpp, "•/.s" , " Mult"); 
f printf (fparam,"7.s"," Mult") ; 

> */ 

f printf (fparam, "\n 7s 7.e" , "CRITERIA = :", value); 
f printf (fparam, "\n7.s 7.e 7.s 7.e %s 7.e" , "x kkml CO] = ", 

x kkml COD , " xkkmlCl] = ", xkkmlCl], 

" xkkmlC2] = " , x kkml C2D ) ; 

fprintf (fparam, "\n7s 7e V.s V.e" , "x kkml C3 ] = ",xkkmlC3], 
" xkkmlC4] = ",xkkmlC4D); 
f printf (fparam, "\n7.s V.e 7.s V.e V.s V.e V.s V.e", 

"rCO] = " , r C01 , " rCl] = M ,rCl], 

" rC2] = " ,r C2] , " rC3] = ",rC3]>; 
f printf (fparam, ''\n'/.s V.e V.s V.e V.s V.e" , "pkkml CO] = ", 
pkkmlCO]," pkkmlCl] = ",pkkmlCl], 

" pkkml C2] = ", pkkml C2] ) ; 
f cl ose (fparam) ; 

f pin=f open ( "tr j . inp " , "w" ) ; 

fprintf(fpin,"7d V.d" ,n array ,runn) ; 
f printf (f pin , "\nXe V.e V.e", 

xkkmlC0],xkkmlC2],xkkmlC4]);/*positions*/ 
fprintf (f pin , "\n7e V.e" , 

xkkmlCl], xkkmlC3]) ; /*veloci ti es*/ 
f close (f pin) ; 
if (option==4) goto quit; 

f ptr j=f open ( "tr j . dat " , "r" ) ; 
f pz i c=f open ( "zi c. dat " , "r " ) ; 

/* INITIALIZATION FOR QFIND */ 
velm=l . O/vel ; 
al4=phi sCO] ; 
a2=al4*al4; 
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g l=a2*0. 5; 
g2=gl*gl; 
g3=al4*gl ; 
sigacc=sacc*sacc; 
sigcc=scc*scc; 

■for (i=0; i<nmax ; i++) 

far ( j=0; j<nmax ; j++) q i i *nmax+ j 3=0. 0; 
qCmaxsq-1 3=si gdi v*a2*si gdi v; 

/* INITIALIZATION FOR HROW */ 
hrowC13=0.0; hrowC33=0.0; 

/* INITIALIZATION FOR PDUM */ 
far <i i =0; i i <nmax ; i i ++) 

i 

j j=i i *nmax ; 
pdumC jj+1 3=0.0; 
pdumC jj+3 3=0. 0; 

> 

pdumC63=l . 0; pdumC 183=1 . 0; 

/* INITIALIZING RR */ 

for <ii=0; ii<4; ii++) rr C i i 3=sqrt <r C i i 3 ) ; 



printf ( "NnPress any key and ENTER to start ... "); 

scanf ("7 s" ,&c) ; 



/* INSERT TIME CHECKING HERE */ 

/* START THE TIME SLOT LOOP AND SET ARRAY HANDOFF */ 
/* POINT */ 

for (kk=0; kk<i time; kk++) 

/* printf ("\n7.s 7.e","t = oldtime); 

printf <"7.s 7.d M ," kk = " ,kk); */ 

dk=kk+l ; 

/* printf < " \n***********************************" ) ;-*/ 

/* printf ("720s 7d \n " , "************* TIME :",kk); */ 

printf ("\nwait "); 
printf ; 

/* GET HYDROPHONE ARRAY COORDINATES */ 
if (narray != 0) 

•C 

if (xkkmlC03 > swO) iS=0; 
else if <xkkmlC03 > swl) i8=12; 

else if (xkkmlC03 > sw2) iS=24; 

else if (xkkmlC03 > sw3) i8=36; 

else if (xkkmlC03 > sw4) i8=48; 
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else iB=60; 

■for (i=0; i<4; i++> 

C 

j=3*i+i8; 
xbCi 3=hydroC j 3 ; 
ybC i 3=hydroC j+1 3 ; 
zbCi 3=hydroC j+23 ; 
> 



/* INSERT TIME CHCKING + WAITING LOOP HERE */ 

gettp: 

/* GET THE TRUE TIMES AND THE TRUE POSITIONS 
WRITE TRUE TIME and TRUE POSITIONS */ 

■fscan-F (-fptr j , "7.1 F 7.1 -f 7.1 F", 

&dataC03 , &dataC 1 3 , &dataC23 ) ; 

-fscan-f (-fpzic, "7.1-f 7.1 -f 7.1 -F 7.1 -f" , 

8<zicC03,&zicC13,&zicC23,&zicC33); 
truedC03=dataC03 ; trued E 1 3=dataC 13; 
truedC23=dataC23 ; 

/* FIRST GET HROW-CALCULATE GAIN , ESTIMATE */ 

/* COVARIANCE OF ERROR BASED ON ONE TIME */ 

/* MEASUREMENT-TC , TX , TY , TZ */ 

adapt: -For (i =0; i < js; i ++) 

< 

x=xkkmlC03-xbCi 3; 
y=xkkml C23-ybCi 3 ; 
z=xkkmlC43-zbCi 3; 

denom=sqrt (x*x+y*y+z*z ) ; 
auxh=velm/denom; 
hrowC03=x*auxh ; 
hrowC23=y*auxh ; 
hrowC43=z*auxh; 

■for < i i =0; i i <nmax ; i i ++) 

i in=i i *nmax ; 

gnumCi i 3=pkkml Ci in3*hrowC03+ 

pkk.ml C i i n+23 *hrowC23+pkkml C i i n+43*hrowC43 ; 

> 

gdenom=hrowC03*gnumC03+hrowC23*gnumC23+hrowC43*gnumC43+r C i 3 ; 

/* THIS IS THE FIRST GAIN COLUMN */ 

For ( i i =0; i i <nmax ; i i ++) gi Ci i 3=gnumCi i 3/gdenom; 

/* printF ("\n VECTOR HROW : \n" >; 
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printvec <hrow,5> ; 

print-f <"\nVECTOR GNUM : \n"); 

printvec <gnum,5) ; 

print-f ("XnDENOMINATOR is : "); 

print-f <""/£e \n " , gdenom) ; 

print-f ("\n VECTOR GI : \n">; 

printvec <gi ,5) ; */ 

/* CALCULATE THE COVARIANCE OF ERROR PI */ 

-for (i i =0; i i <nmax ; i i +=2) 

£ 

pdumCi i ]=<-giCO]#hrowCii]) ; 
pdumC i i +5]= <-gi C 1 ]*hrowCi i ] ) ; 
pdumCi i+10]=(-gi C2]*hrowC i i 3 ) ; 
pdumC ii +151 = <-gi C3]*hrawC i i 3 ) ; 
pdumC ii +20 ] = <-gi C4]*hrawCi i 3 ) ; 

> 

pdumCO]+=l . 0; 
pdumC 12]+= 1 . 0; 
pdumC 24 ]+=l . 0; 

pi CO] =pdumCO]*pkkml C0]+pdumC2]*pkkml C 10] + 
pdumC 4]*pkkml C20] ; 

pi C 1 ] =pdumCO]*pkkml C 1 ]+pdumC2]*pkkml C 1 1 ]+ 
pdumC 4]*pkkml C21 ] ; 

pi C2] =pdumCO]*pkkml C2]+pdumC2]*pkkml C 12]+ 
pdumC4]*pkkml C22] ; 

pi C3] =pdumCO]*pkkml C3]+pdumC2]*pkkmlC13]+ 
pdumC4]*pkkmlC23]; 

pi C4] =pdumCO]*pkkml C4]+pdumC2]*pkkml C 14]+ 
pdumC4]*pkkml C24] ; 
piC5]=piCl]; 

pi C6] =pdumC5]*pkkmlCl]+pkkmlC6]+ 

pdumC7]*pkkmlCll]+pdumC9]*pkkmlC21] 
pi C7] =pdumC5]*pkkml C2]+pkkmlC7]+ 

pdumC7]*pkkml C12]+pdumC9]*pkkmlC22] 
pi CS] =pdumC5]#pkkml C3]+pkkmlCB]+ 

pdumC7]*pkkml C 13]+pdumC9]*pkkml C23] 
pi C9] =pdumC5]*pkkml C4]+pkkmlC9]+ 

pdumC7]*pkkml C 14]+pdumC9]*pkkml C24] 
pi C 10]=pi C2] ; 
piCll]=piC7]; 

pi C 12]=pdumC 10]*pkkmlC2]+pdumC12]*pkkmlC12]+ 
pdumC 14]*pkkml C22] ; 

pi C 13]=pdumC 10]*pkkml C3]+pdumC 12]*pkkmlC13]+ 
pdumC 14]*pkkml C23] ; 

pi C 14]=pdumC 10]*pkkml C4]+pdumC 12]*pkkmlC14]+ 
pdumC 14]*pkkml C24] ; 
pi C 15]=pi C3] ; 
pi C16]=pi CS] ; 
piC17]=piC13]; 
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pi C 183=pdumC 153*pkkml C33+pdumC 173*pkkml C 133 + 
pkkml C 183+pdumC 193*pkkml C233 ; 
pi C 193=pdumC 153*pkkml C43+pdumC 173*pkkmlC143+ 
pkkml C 193+pdumC 193*pkkml C243 ; 
pi C203=pi C43 ; 
pi C21 3=pi C93 ; 
pi C223=pi C 143 ; 
pi C233=pi C 193 ; 

pi C243=pdumC203*pkkml E43+pdumC223*pkkml C 143+ 
pdumC243*pkkml C243 ; 

/* CALCULATE FIRST MEASUREMENT PREDICTION */ 
zhat=vel m*denom; 
zdi-f-fCi3=zicCi 3-zhat ; 

/* COMPUTE THE GATE FOR ERRONEOUS TIME */ 

/* MEASUREMENTS */ 

pkl= (pi C03 < 0) ? -p i C 0 3 : piC03; 

pk3= (pi C 123 < 0) ? -pi C 123 s piC123; 

pk5= (pi C243 < 0) ? -pi C243 s piC243; 

pi 1= (pkl > pk3) ? pkl : pk3; 

p = <pll > pk5) ? pll s pk5; 

/* pgate=sqrt (p) *velm;*/ 

x=p; 

i-f (x .'= 0.0) 

i-f (x < 1.0) x = 1.0; 
do 

•C 

ol dx=x ; 

x=( x + p/x )*0.5; 
temp=ol dx-x ; 

rabs=(temp < 0) ? -temp : temp; 

> 

while (rabs > 0.0); 

> 

pgate=x*velm; 

gate=3. 0* <pgate+rr Ci 3 ) ; 

/* print-f ("XnMATRIX PDUM : \n"); 

printmat <pdum,5,5) ; 
print-f ("\nMATRIX PI : \n M ); 
printmat (pi ,5,5) ; 
print-f ("\nZDIFF is : " ) ; 

print-f <"\7.e \n " , zdi -f -f Ci 3 ) ; 
pri ntf < " \nGATE is s "); 
print-f <"\7.e \n" ,gate); */ 
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/* EDIT INVALID TIME MEASUREMENTS */ 
zgate= (zdi -f -f C i 3 < 0) ? -zdi-f-fCi] : zdi-f-fCil; 

i-f (zgate >= gate) 

£ 

■for < i i=0; i i <nmax ; i i++) gi£iil=0.0; 
zdi-f-f Ci:=999.0; 

/* TAG FOR INVALID TIME MEASUREMENT */ 

> 

/* CALCULATE ESTIMATE BASED ON ONE */ 

/* MEASUREMENT PREDICTION */ 

■for ( i i =0; i i <nmax ; i i ++) 

xi Cii]=xkkml£ii]+gi Cii3*zdi-f-fCi]; 

/* print-f <"\n VECTOR XI : \n"); 

printvec (xi ,5) ; */ 

■for ( i i =0; i iCnmax ; i i ++) 

£ 

xkkmlCii]=xiCii]; 

pkkmlCii3=piCii]; 

> 

■for ( j j=nmax ; j j<maxsq; j j++) pkkmlCjj]=piCjj3; 
i-f (i == 3) goto 156; 

} /* end JS loop */ 

/* NOTE : CALLED ORIGINAL X (0/-1 ) , XKKM1 . */ 

/* UPDATED AFTER 1 MEASUREMENT CALLED IT XI,*/ 

/* THEN MADE XKKM1 AND WENT THRU ITERATION */ 

/* AGAIN. AFTER YOU HAVE UPDATED XKKM1 FOR */ 

/* EACH MEASUREMENT XKK=XI AND PKK-PI. */ 

156: -for < i i =0; i i <nmax ; i i ++) 

£ 

xkkCiil=xiCiiI; 
pkkC i i ]=pi Ci i ] ; 

> 

■for < j j=nmax ; j jCmaxsq; j j++) 

£ 

pkkC j j]=pi C j j □ ; 

> 

/* print-f ("NnUPDATING VECTORS INTERVAL ... \n"); 

print-f ("\n VECTOR XKKM1 : \n">; 
printvec (xkkml ,5) ; 
print-f ("\nMATRIX PKKM1 : \n"); 
pr i nt mat (pkkml , 5 , 5) ; 



print-f ("XnVECTOR XKK : \n">; 

printvec (xkk ,5) ; 

print-f ("NnMATRIX PKK : \n"); 
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print mat (pkk ,5,5) ; 



*/ 



/* RECALCULATE TIME MEASUREMENTS, and FORM */ 
/* ABSOLUTE VALUE OF RESIDUALS. */ 
■for <i=0; i<4; i++) 



/* EDIT INVALID TIMES FOR ADAPTIVE MANEUVER */ 
/* ROUTINE */ 

■C 

if <zdiffCi3 < 999.0) 

■C 

x=xkkmlC03-xbCi3; 
y=xkkml C23-ybCi I ; 
z=xkkmlC43-zbCi 3; 

/* denom=sqrt (x*x+y*y+z*z ) ; */ 

denom=x*x+y*y+z*z ; 

x=denom; 

if (x ! = 0.0) 

C 

if (x < 1.0) x = 1.0; 
do 

{ 

oldx=x ; 

x=( x + denom/x )*0.5; 
temp=ol dx-x ; 

rabs=(temp < 0) ? -temp : temp; 

> 

while (rabs >= l.e-12); 

> 

denom=x ; 

zhat=velm*denom; 
temp=zicCi 3-zhat; 

zdi f f t i 3= ( temp < 0) ? -temp : temp; 

> 

el se 

zdiffCi 3=0.0; 
nzdi f f — =1 ; 

> 



/* 

if 



THIS FUNCTION COMPUTES THE ADAPTIVE Q MATRIX 
(i curve == 1) 

bb=xkkmlC33*xkkmlC33; 
dd=x kkml C13*xkkmltl3; 
al=bb+dd ; 



*/ 
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bd=xkkml C 1 3*xkkml C33 ; 

el=dd*si gacc/al+bb*si gcc; 

el2=bd*si gacc/al-bd*si gcc; 

e2=bb*sigacc/al+dd*si gcc; 

qC03=g2*el ; 

qC 1 3=g3*el ; 

qC23=g2*el2; 

qC33=g3*el2; 

qC63=a2*el $ 

q[73=qC33 ; 

qC83=a2*el2; 

qC 123=g2*e2; 

qC 133=g3*e2; 

qC183=a2*e2; 

> 

else 

bb=gammaC03*gammaC03 ; 
dd=gammaC 1 3*gammaC 13; 
qC03=bb*covwC03 ; 
qC 1 3=gammaC03*covwL03*gammaC 13; 
q [23=0.0; q [33=0.0; 
q[63=dd*covw[03 ; 
q [73=0. 0; q[83=0.0; 
q[ 123=bb*covw[ 13; 

q[ 133=gamma[03*covw[ 1 3*gammaC 1 3 ; 
q[ 183=dd*covw[ 13; 

> 

q[53=q[ 1 3 ; q[103=q[23; q[113=q[73; 

q[ 153=qC33 ; q[163=q[83; q[173=q[133; 

/* printf ("NnMATRIX Q s \n"); 

printmat <q,5,5) ; */ 

/* IF ALL TIME MEASUREMENTS EXCEED GATE, BYPASS */ 
/* ADAPTIVE MANEUVER ROUTINE. */ 

if (nzdiff != 0.0) 

zdi f av= (zdiff[03+zdiff[13+zdiff[23+zdiff[33)/nzdiff; 

/* print-f ("\nZDIFAV is : "); 

printf ("\/Ce \n", zdifav); */ 

/* DO NOT PERFORM ADAPTIVE MANEUVER IF */ 

/* FILTER HAS NOT ACHIEVED THE STEADY STATE. */ 
i-f (kk > 3) 



/* IF ZDIFAV MEETS CRITERIA TRANSFER OUT /* 
/* OF ADAPTIVE MANEUVER ROUTINE. */ 
if (zdifav > value && icont < 5) 
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c 

/* INCREASE BAIN */ 
for ( j=0; j<maxsq; j++) 

pkkmlCj]=pkkCj]+qCj]; 

/* printf ("\n INCREASE SAIN AND PERFORM"); 

printf (" ADAPTIVE ... \n"); 
printf ("\nMATRIX PKKM1 : \n"); 
printmat (pkkml , 5,5) ; */ 



/* PERFORM ADAPTIVE MANEUVER BY */ 

/* REITERATING SAME TIME 

SLOT. 



*/ 



skip: 



i cont+=l ; 
goto adapt; 

} 

> 

> 

icont=0; 
nzdi f f =4; 

xdif f CO]=xkkCO]-truedCO] ; 
xdi f f C 1 ]=xkkC2]-truedC 1 ] ; 
xdif f C2]=x kkC4]-truedC2] ; 



/* temp=xdi ffCl]*xdiffCl]/pkkmlC12]; 

if (temp < limvar) icurve=0; 
else icurve=l; */ 



f printf (fpe, "\n V.d V.e V.e V.e" , 

kk,xdiff CO], xdif f Cl 3, xdif f C2D) ; 



/* WRITE */ 

/* printf ("\nVECTOR ERROR : XDIFF YDIFF ZDIFF\n" ) 

pri ntvec (xdi f f , 3) ; */ 

/* PREDICTIONS FOR PKKM1 */ 



for <i =0; i<nmax ; i ++) 

C 

phipkkCi ]=pkk Ci ]+pkkCi+5]*phisC0]; 
phipkkCi+5]=pkkCi+5] ; 

phipkkCi+10]=pkkCi+10]+pkkCi+15]*phisC2]; 
phipkkCi+15]=pkkCi+15]; 
phipkkCi+20D=pkkCi+20] ; 

> 



for (i=0; i<nmax ; i++) 

C 

i n=i *nmax ; 

pktemp Cin]=phipkkCin]+phipkkCin+l]*phisCO]; 



1SS 



pktempCin+1 3=phipkkCin+l 1 ; 

pktempCin+23=phipkkCin+23+phipkkCin+3]*phi sC23 
pktempCin+33=phipkkCin+33; 
pktempCin+43=phipkkCin+43 ; 

> 



■for ( j=0; j<maxsq; j++) pkkml C j 3=pktemp C j 3+qC j 3 ; 

/* print-f ( "\nPREDICTIONS FOR PKKM1 ... \n"); 

print-f ("\nMATRIX PKKM1 : \n"); 
pr i ntmat (pkkml , 5, 5) ; */ 

/* CALCULATE NEW XKKM1 */ 

■for ( i i -0; i iCnmax ; i i ++) 

xkkmlCii3=xkkCii3; 

/* pl=sqrt (-fabs (pkkCi i 3 ) ) ; */ 

> 

x kkml C03=xkkml C03+phi sC03*xkkC 1 3 ; 
xkkml C23=xkkml E23+phi sC23*xkkC33 ; 

/* printf ( "\nNEW XKKM1 ... (-for use in next time"); 

print-f (" interval ) \n") ; 
print-f <"\n VECTOR XKKM1 s \n"); 
printvec (xkkml ,5) ; */ 

f printf (fpp, "\n 7. d 7.e 7.e 7.e" , 

kk,pkkC03,pkkC123 , pkkC243 ) ; • 

-fprint-f (-fpxyz , "\n "Ad Y.e %e 7.e" , 

kk,xkk[03,xkkC23,xkkC43) ; 



} /* end TIME loop */ 

f close (fpxyz ) ; 
f close (f pe) ; 
f cl ose (f pp ) ; 
f close (f pzic) ; 
f cl ose (f ptr j ) ; 

printf ( "press a key and ENTER to continue ..."); 
scanf <"%s" ,&c> ; 

qui t : 

printf ("\nquit ? CY/N3 :"); 
scanf ("7.s" ,&c) ; 

if <c== 'n ' !! c== ' N ' ) goto menu; 

} /* end MAIN PROGRAM */ 

/* 
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printvec (vector ,dim) 
int dim; 

doable vectorC3; 
int i, j; 

•far < j=0; j<di m; j++) printfC'Xe Xls" , vector Cj 3 , " "); 
print-f ( " \n " ) ; 

> 

printmat (matrix , row, col ) 
int row, col; 
double matrix C 3; 

i 

int i,j; 

for ( i =0; i <row; i++) 

for ( j=0; j<col ; j++) printf("7.e 

XI s" , matr ix Ei *col + j 3 , " "); 
printf ( "\n" ) ; 

> 

> */ 
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tra jec (narray , hydro, xb ,yb , zb ,x kkml , j start , jend , wd , g , worg ) 
double hydro C 1 ,xbCl,ybC3,zbC3,xkkmlCl,wd,g; 
int narray , worg , jstart , jend ; 

£ 

int pli,i, start , end ; 

double ppl ,r 1 ,sqrt () ,zcs,zicC4],ziC43; 
double xd ,yd ,zd , v,den , velm,gg; 

static double xnC6i3 = £-3. 01 , -2.0, -1.5, 1.0, -0.6, 

0. 0J ; 

static double ynC61 = 'C0. 0, 0.0228, 0.0668, 0.1357, 

0.2743, 0.51; 

static double snC51={43. 8596, 11.3636, 7.25689, 

2.891352, 2. 65887> ; 

double dataC53,wr; 
int runn,i8; 

FILE *fptrj, *fopen(); 

FILE *fpzic, *fopen(); 
int j,jj; 

/* THIS ROUTINE COMPUTES TRUE TRAJECTORY OF TORPEDO 
dataC03=true x position, dataCll= true y position 
dataC23=true z position, w=turn rate 
dataC33=x_veloci ty , dataC4]=y_vel oci ty 
jstart=time slot where maneuver starts 

jend=time slot where maneuver ends */ 

dataC01=xkkml CO! ; dataCl 3=xkkml C23 ; dataC23=xkkml C43 ; 
dataC31=x kkml CIO; dataC43=xkkml C33 ; vel m=l . 0/vel ; 
r 1=0. 49; 

wr=twopi *wd/360. 0; gg=32.2*g; start= jstart ; end=jend; 

pr i ntf ( " \n */.s \n","start execution o-f trj.c"); 
f ptr j=f open ( "tr j . dat " , "w" ) ; 
f pz i c=f open ( "zic. dat " , "w" ) ; 

■for ( j j=0; j j< 150; j j++) 

/* GET HYDROPHONE ARRAY COORDINATES */ 
i-f (narray != 0) 



if (dataCOl > swO) 


■ *- 

o 

II 

CD 

•H 




el se 


if 


(dataCOl 


> 


swl ) 


i 8=12; 


el se 


if 


(dataCOl 


> 


sw2) 


i 8=24; 


el se 


if 


(dataCOl 


> 


sw3) 


i 8=36; 


else 


if 


(dataCOl 


> 


sw4) 


i8=48; 


el se 


i 8= 


60; 








for 


o 

II 

•H 


; i < 4 ; i ++ ) 









i 

j=3*i+i8; 
xbCi ]=hydroC j]; 
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yb C i D=hydroC j + 1 D ; 
zbCi D=hydroC j+2D ; 

> 

> 

/* COMPUTE THE TRUE TIMES AND THE TRUE POSITIONS 
WRITE TRUE TIME and TRUE POSITIONS */ 
printf <"*/.s",". "); 

-fprint-f (t ptr j , "\n V.e V.e V.e" , 

data COD , dataC 1 1 , dataC2D ) ; 



for ( i =0; i <4; i ++) 

< 

xd=dataCOD-xbCi 3 ; 
yd=dataC 1 3-yb Ci 3 ; 
zd=dataC23-zbCi 3 ; 
den=sqrt (xd*xd+yd*yd+zd*zd) ; 
ziCi 3=velm*den; 

/* print-f ("\nTRUE TIMES s \n"); 

pri ntvec (zi ,4) ; 

printf ("\nTRUE POSITIONS : \n">; 
pri ntvec (data , 3) ; */ 

/* GET NOISE, SCALE AND ADD TO TRUE */ 

/* MEASUREMENT TIME */ 

pp 1=317. 0*r 1 ; 

pli=ppl; 

rl=ppl-pli ; 

v=r 1 ; 

j=0; 

it (v > 0.5) v-l.O-rl; 
r2: it (v < ynCj+13) goto r8; 
j+=l; 
goto r2; 

r8: v= ( v-ynC j 3 ) *sn C j 3+xn C j 3 ; 
it (rl >= 0. 5) v=(-v) ; 

zcs= ( v/3. 00) *0. 00001 ; 

/* printt ("\nRANDOM NUMBER : " ) ; 

printtC'/ie \n",zcs) ;*/ 
z i cC i ]=zcs+z i C i 3 ; 

> 

tprintt (tpzic, "\n V.e V.e V.e V.e" , 

zicC03,zicC13,zicC23,zicC33) ; 

/* NEW DATA */ 

1 i ne ( j j , 0 , 150 , data) ; 



> 
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•fclose (fptrj ) ; 

-fclose (-fpzic) ; 

> 

1 ine ( j , start , end, data) 
double dataCl; 
int j, start, end; 

£ 

i-f ( ( j >= start) && ( j < 

£ 

dataCO]+=l . 31*dataC3] 
dataC 1 3+=l . 31*dataC43 
> 



end) ) 

m 

9 

■ 

9 
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APPENDIX G 



LISTING OF PLOT. C - PLOTTING ROUTINE 
/********■#*********************#**#************■*•)<•■*•*•#■**■#■■#•■*••#■#/ 



/* 

/* 

/* 



PLOTTING ROUTINE - FILE : PLOT.C (IBM-PC) 



*/ 

*/ 

*/ 



/****************************************■******************/ 



#i ncl ude 
#i ncl ude 
#include 
#i ncl ude 
mai n ( ) 
f 



"stdio. h" 
"math. h " 
"limits. h" 
"kalman. h" 



f 1 oat i x [npt 3 , var [npt 3 , sum , sumsq ; 

int i , col , row, inc ,c , tt ,choi ce,narray,runn , op ,cc; 
float xCnptl, yCnptl, z Cnpt 3 , t Cnpt 3 ,xx , yy , zz ; 
f 1 oat xmax , xmi n , ymax , ymin , power , yaux ; 
f 1 oat hei ght , wi dth , xscal e , yscal e; 
float ymax t , ymi nt , xmint , xmax t ; 
double xkkmlC53; 

FILE *fpt, *fopen(); 

FILE *fpxyz, *fopen(); 

FILE *fpe, *fopen(); 

FILE *fpp, *fopen(); 

FILE *fpin, *fopen(); 

f pi n=f open ( " tr j . i np " , "r") ; 
f scanf (f pin, "Xd %d" , ?<n array , &runn ) ; 
fscanf (fpin, "7.1f '/.If ‘/.If */.lf '/.If", 

&xkkmlC03,&xkkmlC23 ,&xkkmlC43 ,&xkkmlC13 , 
?<xkkml C33 ) ; 
f close (fpin) ; 



CLR_SCRN; 
pri ntf ( " \n 
printf ( "Xd" ,runn) ; 
puts ( " \n \n " ) ; 
if (narray==0) 
printf ( " \n 
el se printf ( " \n 
printf ( "\n V.s 
xkkml [03 , 
xkkml C43) ; 
printf ("\n Xs %e 7 . s 



INITIAL CONDITIONS FOR RUN # " ) 



CASE is SINGLE ARRAY."); 
CASE is MULTIPLE ARRAY."); 
■/.e %s 7.e. 7.s V.e \n"," xpos = 

ypos = ", xkkml C23, " zpos = ", 



V.e \n ' 



x vel = 



xkkml [13 



yvel = " ,xkkml[33 ) ; 



choice=l ; 
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print-f ( "\n\n\n\n\n\n\n\n\n\n\nPress any key to"); 
print-f<" continue ... "); 

c=keyrd ( ) ; 

■for ( ; ; ) 

CLR_SCRN; 

print-f (" PLOT OPTIONS : "); 



print-f ( "\n" ) ; 












print-f ( "\n 


1) 


X_EST 


VS 


TIME_SL0T 


") ; 


print-f ( " \n 


2) 


Y_EST 


vs 


TIME_SL0T 


") ; 


print-f < "\n 


3) 


Z_EST 


vs 


TIME SLOT 


") ; 


print-f < "\n 


4) 


Y EST 


vs 


X_EST " ) ; 




print-f < "\n 


5) 


X_ERR 


vs 


TIME_SL0T 


") ; 


print-f <"\n 


6) 


Y ERR 


vs 


TIME_SL0T 


") ; 


print-f ( "\n 


7) 


ZJERR 


vs 


TIME_SL0T 


") ; 


print-f ( "\n 


8) 


X_VAR 


vs 


TIME_SL0T 


") ; 


print-f <"\n 


9) 


Y VAR 


vs 


TIME_SL0T 


") ; 


print-f ( " \n 


10) 


Z VAR 


vs 


TIME SLOT 


") ; 


print-f <"\n 


11) 


PKK X 


vs 


TIME SLOT 


"> ; 


print-f ( "\n 


12) 


PKK_Y 


vs 


TIME_SL0T 


") ; 


print-f ( "\n 


13) 


PKK_Z 


vs 


TIME_SL0T 


") ; 


print-f < " \n" ) ; 












print-f <"\n 


14) 


TRUE TRAJECTORY " ) 


• 

j 


print-f ("\n 


15) 


CHANGE 


: PLOT Cdot <- 


> linear") 


print-f (" (de-fault 


=1 inear) 1 


1 ") ; 






print-f ( "\n 


16) 


EXIT 


"); 






print-f ( "\n" ) ; 












print-f ( "\n 




enter 


value (1-16) 


s ") ; 



mops CUR_SAVE; 

scan-f ( "%2d" , Stop) ; 
i-f (op<l !! op > 1 6 ) 

C 

BELL; 

CUR_REST ; 
goto mop; 

y 

pri nt-f < "\nXsXdXs" , " option is ",op, 

" CY/NI s "); 
scan-f < "/is" , &cc ) ; 
i-f (cc ! =* ' y ' &«< cc ! = ' Y ' ) 

£ 

BELL; 

CUR_REST ; 
goto mop; 

> 

print-f ("\n wait "); 

switch (op) 
case 1 : 
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{ 

f pxyz=f open ( "xkk. dat " , "r" ) ; 

f scanf (fpxyz , "\n 7.d V.i V.i V.i " , &tt , &xx ,&yy ,&zz > 
x COD=xx ; tCOD=tt; 

ymax=xx; ymin=xx; xmax=npt; xmin=0.0; 

■for (i=l ; i<npt ; i++) 

C 

f scan-f (fpxyz, "\n 7.d 7.-f V.i V.i", 

&tt , Stxx , &yy , &zz ) ; 
xCiD=xx; tCi3=tt; 

ii (xx>ymax) ymax=xx; 
else if(xx<ymin) ymin=xx; 

printf ("%s" ") ; 

> 

plot (ymax , xmax , x mi n,ym in, x,t, choice) ; 
biosset (3,30) ; 

printf (" 7.s ","X_EST vs TIME_SLQT" ) ; 

biosset (4,8) ; 

print-f ( "7s" , " (FT) ") ; 

fclose (fpxyz ) ; 

break ; 

J 

case 2: 

C 

f pxyz =f open ( “xkk. dat" , "r " ) ; 

-fscan-f (fpxyz , "\n 7.d 7.-f 7.-f 7.-f " , &tt ,&xx , &yy , &zz ) 
x C03=yy ; tC03=tt; 

ymax=yy; ymin=yy; xmax=npt; xmin=0.0; 

■for ( i =1 ; i<npt ; i++) 

C 

-fscan-f (-fpxyz , ”\n 7.d V.i V.i V.i ", 

&t t , &x x , &yy , &z z ) ; 
x Ci D=yy; tCil=tt; 

if (yy>ymax) ymax=yy; 
else if (yy<ymin) ymin=yy; 

printf ( "7s" ,"."); 

> 

plot (ymax , xmax ,xmin,ymin,x,t, choice) ; 
biosset (3,30) ; 

printf (" 7.s ","Y_EST vs TIME_SL0T" ) ; 

biosset (4,8) ; 

printf ("7. s" , " (FT) ") ; 

fclose (fpxyz ) ; 

break ; 

> 

case 3: 
f 
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fpxyz=fopen ("xkk.dat" , "r"); 

f scan-f (f pxyz , " \n V.6 V.-f V.-f V.-f " , &tt , &xx , &yy , &zz ) 
xCOD=zz; tC03=tt; 

ymax=zz; ymin=zz; xmax=npt; xmin=0.0; 

■for ( i =1 ; i <npt ; i++) 

i 

fscanf (fpxyz , "\n V.d V.-f V.-f */.f " , 

&tt , &xx , &yy , &zz ) ; 
x Ci 3=zz ; tCi]=tt; 

if (zz>ymax) ymax=zz; 
else if(zz<ymin) ymin=zz; 

pri nt-f ( "7s" ,"."); 

> 

plot (ymax , xmax , xmin , ymin ,x,t, choice) ; 
bi osset (3, 30) ; 

printf ( " 7.s ","Z_EST vs TIIiE_SLOT" ) ; 

bi osset (4,9) ; 

print-f ("7.s", " (FT) " ) ; 

fclose (fpxyz ) ; 

break ; 

> 

case 4: 

C 

f pxyz =f open ( "xkk. dat " , "r " ) ; 

fscanf (fpxyz , "\n V. d V.-f V.-f '/.f " , &tt , &xx , &yy ,&zz ) 
tC03=xx; xC03=yy; 

ymax=yy; ymin=yy; xmax=xx; xmin=xx; 
for (i=l; i<npt; i++) 

< 

f scanf (f pxyz , "\n 7.d */.f V.-f 7.f " , 

&tt , &xx , &yy , &zz ) ; 
tCil=xx; xCi]=yy; 

if (yy>ymax) ymax=yy; 
else if (yy<ymin) ymin=yy; 

if (xx>xmax) xmax=xx; 
else if (xx<xmin) xmin=xx; 

printf ( "7s" ,"."); 

> 

plot (ymax , xmax ,xmin, ymi n , x , t , choi ce) ; 
bi osset (3,30) ; 

printf (" 7.s " ,"Y_EST vs X_EST"); 

biosset (4,8) ; 

printf ("7.s" , " (FT) " ) ; 

biosset (22,63) ; 

printf ("7.s" , " (FT) " ) ; 

fclose (fpxyz ) ; 
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break ; 

> 

case 5: 

■C 

f pe=f open ( "xdif f . dat" , "r " ) ; 

f scanf (fpe, "\n 7.d 7.f 7.f 7.f " , &tt , &xx , &yy , &zz ) 
x C01=xx ; tC03=tt; 

ymax=xx; ymin=xx; xmax=npt; xmin=0.0; 

■for (i=l ; i<npt ; i++) 

fscan-f (fpe, "\n 7.d 7.f 7.f 7.f " , 

&t t , &x x , &yy , &z z ) ; 
x Ci 3=xx ; tCi l=tt ; 

if (xx>ymax) ymax=xxj 
else if(xx<ymin) ymin=xx; 

printf ("7.s" ,"."); 

J 

plot (ymax , xmax , xmi n , ymi n,x,t, choice) ; 
bi osset (3,30) ; 

printf (" 7.s ","X_ERR vs TIME_SLQT" ) ; 
biosset (4,8) ; 
printf ("7.s" , " (FT) ") ; 
f cl ose (fpe) ; 
break ; 

> 

case 6s 

i 

fpe=fopen("xdiff .dat", "r") ; 

f scanf (fpe, "\n 7.d 7.f 7.f 7.f " ,&tt,&xx ,&yy,&zz ) 
x C03=yy ; tC01=ttj 

ymax=yy; ymin=yy; xmax=npt; xmin=0.0; 
for (i=l ; i<npt; i++) 

•C 

f scanf (fpe, "\n */.d 7.f 7.f 7.f " , 

&tt , &xx ,&yy , &zz ) ; 
xCil=yy; tCil=tt; 

if (yy>ymax) ymax=yy; 
else if (yy<ymin) ymin=yy; 

printf ("7.s" , " . " ) ; 

plot (ymax , xmax , xmi n , ymi n , x , t ,choi ce) ; 
biosset (3,30) ; 

printf (" 7.s " , " Y_ERR vs TIME_SL0T" ) ; 
biosset (4,8) ; 
printf ("7.s" , " (FT) " ) ; 
f cl ose (fpe) ; 
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break; 

> 

case 7: 

£ 

f pe=f open ( "xdi -f -f . dat " , "r " ) ; 

-fscan-f (-fpe, "\n- V.d VA VA VA " , &tt , 8<xx , &yy , &zz ) ; 
x£03=zz; tC03=tt; 

ymax=zz; ymin=zz; xmax=npt; xmin=0.0; 

■for ( i=l ; i<npt ; i++) 

£ 

■fscan-f (-fpe, "\n V.d VA VA VA " , 

&tt , &xx , &yy , &zz ) ; 
xCi3=zz; t£i3=tt; 

i-f (zz>ymax) ymax=zz; 
else i-f(zz<ymin) ymin=zz; 

print-f <"7.s",". ") ; 

> 

plot (ymax , xmax , xmin , ymin , x , t , choi ce) ; 
biosset (3,30) ; 

print-f (" */.s ","Z_ERR vs TIME_SLQT" ) ; 
biosset (4,8) ; 
print-f ("7.s" , " (FT) ") ; 

■f close (-fpe) ; 
break; 

y 

case 8: 

£ 

■f pe=-f open ("xdi-f-f. dat " , "r") ; 

-fscan-f (-fpe, "\n V.d VA VA VA " , &tt , &xx ,&yy , 8<zz ) ; 
x£03=xx; tC03=tt; 

var £03=0.0; sum=0.0; sumsq=0.0; 

ymax=xx; ymin=xx; xmax=npt; xmin=0.0; 

-for ( i = 1 ; i <npt ; i ++) 

£ 

-fscan-f (-fpe, "\n V.d VA VA VA " , 

&t t , &x x , &yy , &zz ) ; 
x£i3=xx; t£i3=tt; 

sum+=x£i3; sumsq+=x Ci 3*x C i 3 ; 

var Ci 3 = ( (tCil + 1.0) -*sumsq-sum*sum) / ( (t£i3 + 1.0)*t£i3) 
xx=var C i 3 ; 

i-f (xx>ymax) ymax=xx; 
else i-f (xx<ymin) ymin=xx; 

print-f ("7.s" ,"."); 

> 

pi ot ( ymax , xmax , xmin , ymin , var , t , choi ce) ; 
biosset (3,30) ; 
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printfC' 7.s 'V'X.VAR vs TIME_SLOT" ) ; 

bi osset (4,7) ; 

printf <"7.s H , " (FTsq) " ) ; 

f close (fpe) ; 

break; 

case 9: 

f pe=f open ( "xdi f f . dat " , "r") ; 

fscanf (f pe, "\n 7.d 7.f 7.-F 7.-F " , &tt , 8<xx , &yy , &zz ) ; 
x COD=yy; tCOD=tt; 

var C01=0.0; sum=0.0; sumsq=0.0; 

ymax=yy; ymin=yy; xmax=npt; xmin=0.0; 

•for (i = 1 ; i <npt ; i ++) 

-fscan-f (-fpe, "\n 7.d 7.f V.i V.i " , 

8<tt , &xx , &yy ,8<zz ) ; 
xCil=yy; tCil=tt; 

5um+=x [ i ] ; sumsq+=x Ci l*x C i 3 ; 

var C i 1= ( ( t C i 1+1 . 0) *su(nsq-sudi*sum) / ( (tEil+1.0)*tCil) 
yy=var [ i 3 ; 

i-f (yy>ymax) ymax=yy; 
else if (yy<ymin) ymin=yy; 

print-f ( "7s" ,"."); 

> 

plot (ymax , xmax , xmin , ymi n , var , t , choi ce) ; 
bi osset (3,30) ; 

print-f (" 7.s ","Y_VAR vs TIME_SL0T" ) ; 

bi osset (4,7) ; 

print-f ("7.s" , " (FTsq) ") ; 

-f cl ose (fpe) ; 
break; 

case 10: 

•C 

f pe=f open ( "xdif f . dat" , "r " ) ; 

f scanf (f pe, M \n 7.d 7.f 7.f 7.f " , &tt , &xx , &yy , &zz ) ; 
xC03=zz; tC01=tt; 

varC01=0.0; sum=0.0; sumsq=0.0; 

ymax=zz; ymin=zz; xmax=npt; xmin=0.0; 
for (i=l ; i<npt; i++) 

•C 

fscanf (fpe, "\n 7.d 7.f 7.f 7.f " , 

&tt , &xx , &yy , &zz ) ; 
xCil=zz; tCil=tt; 

sum+=xCiD; sumsq+=x Ci ]*x C i 1 ; 
var C i 1= ( (tCil + 1.0) -*sumsq-sum-*sum) / ( (tCil+1.0)*tCi 3) 
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zz=varCi 3 ; 

if (zz>ymax) ymax=zz; 
else if(zz<ymin) ymin=zz; 

printf ("*/.s" ,"."); 

> 

plot (ymax , xmax , xmin,ymin , var , t , choice) ; 
biosset (3,30) ; 

print-f (" V.s " , "Z_VAR vs TIME_SL0T" ) ; 

biosset (4,7) ; 

print-f ("7.s" , " (FTsq) ") ; 

fclose (-f pe) ; 

break ; 

> 

case 11: 

•C 

f pp=f open ( "pkk. dat" , "r" ) ; 

f scanf (f pp , "\n V.d V.i V.i V.i " , &tt , &xx , &yy , &zz ) ; 
xC03=xx; tC03=tt; 

ymax=xx; ymin=xx; xmax=npt; xmin=0.0; 
far (i=l ; i<npt ; i++) 

£ 

f scanf (fpp, "\n V.d V.i V.i V.i ", 

&tt , &xx , &yy,&zz) ; 
x Ci 3=xx ; tCi 3=tt; 

if (xx>ymax) ymax=xx; 
else if(xx<ymin) ymin=xx; 

printf ("7.s" 

plot (ymax , xmax , xmi n , ymin , x , t , choi ce) ; 
biosset (3,30) ; 

printf (" V.s ","PKK_X vs TIME_SLQT" ) ; 

biosset (4,7) ; 

printf ("7.s", " (FTsq) ") ; 

f cl ose (f pp ) ; 

break; 

> 

case 12: 

£ 

f pp=f open ( "pkk. dat" , "r " ) ; 

f scanf (fpp, "\n V.d V.i V.i V.i " , &tt , &xx , &yy , &zz ) ; 
xC03=yy; tC03=tt; 

ymax=yy; ymin=yy; xmax=npt; xmin=0.0; 
for ( i =1 ; i <npt ; i ++) 

£ 

f scanf (fpp, "\n V.d V.i V.i V.i", 

&tt , &xx , &yy , &zz ) ; 
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x£il=yy; tCi]=tt; 

if (yy>ymax) ymax=yy; 
else if(yy<ymin) ymin=yy; 

printf ( "7. s" , " . ” ) ; 

> 

plot (ymax , xmax , x mi n,ymin,x,t, choice) ; 
biosset (3,30) ; 

print-f <" 7.s " , "PKK_Y vs TIME_SL0T" ) ; 

biosset (4,7) ; 

print-f ("7.s" ," (FTsq) ") ; 

fclose (fpp) ; 

break ; 

> 

case 13: 

£ 

-fpp=-f open ( "pkk. dat" , "r " ) ; 

-f scan-f (-f pp , " \n 7.d 7.f V.i 7.-f " , Jett ,&xx , &yy , &zz ) 
x E0D=zz ; t C 01=tt ; 

ymax=zz; ymin=zz; xmax=npt; xmin=0.0; 

-for (i = l ; i<npt ; i ++) 

£ 

i scant (fpp, "\n 7.d V.i V.i 7.f " , 

&t t , &x x , &yy , &z z ) ; 
xCil=zz; t£il=tt; 

i-f (zz>ymax) ymax=zz; 
else i-f (zz<ymin) ymin=zz; 

print-f ( , '7.s" ") ; 

> 

plot (ymax ,xmax ,xmin ,ymin , x , t , choice) ; 
biosset (3,30) ; 

print-f (" 7.s " , " PKK 2 vs TIME_SLQT" ) ; 

biosset (4,7) ; 

print-f ( "7s" , " (FTsq) ") ; 

fclose (fpp) ; 

break ; 

> 

case 14: 

£ 

f pt=f open C'trj. dat" , "r") ; 
f scanf (f pt, "\n 7.f 7.f 7.f " , &xx , &yy , &zz ) ; 
x£01=xx; y£0D=yy; 

ymax=yy; ymin=yy; xmax=xx; xmin=xx; 
for (i=l ; i<npt ; i++) 

£ 

f scanf (fpt, "\n 7.f 7.f 7.f " ,&xx ,&yy ,&zz ) ; 
x£il=xx; y£il=yy; 
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if (yy>ymax) ymax=yy; 
else if (yy<ymin) ymin=yy; 

if (xx>xmax) xmax=xx; 
else if (xx<xmin) xmin=xx; 

printf ("7.s", ") ; 

plot (ymax , xmax ,xmin , ymi n , y ,x , choi ce) ; 
biosset (3,30) ; 

printf (" 7.s ","TRUE_Y vs TRUE_X"); 

biosset (4,8) ; 

printf ("7.s" , " (FT) ") ; 

biosset (22,63) ; 

printf ("7.s" , " (FT) " ) ; 

f close (f pt) ; 

break; 

> 

case 15: 

£ 

printf ("\n CHANGE PLOT ( enter option )") 

printf ("Xn") ; 

printf ("\n 1) LINEAR INTERPOLATION BETWEEN") 

printf (" POINTS"); 

printf ("\n 2) SIMPLE DOT"); 

printf ( "\n") ; 

printf ("\n ENTER choice : "); 

scanf ( "7d " , &choi ce) ; 
goto skip; 
break; 

case 16: goto ending; 
default: 

£ 

goto skip; 
break ; 

> 

> /* end case */ 

CUR_MV (4,67) ; 

printf ("7s 7.d","# ",runn); 

CUR_MV (5,67) ; 

if (narray==0) printf ("SINGLE") ; 
else printf ("MULTIPLE") ; 

c=keyrd ( ) ; 
biosini (3) ; 
skip: 

> /* end for — forever-loop */ 



203 



ending: 



> 



printf ( "\n" ) ; 

print-f <" \nEnd plotting ... Press any key"); 
c=keyrd ( ) ; 

CLR_SCRIM; 
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/**•**•*****■*************************************■*********■**■*/ 



/* */ 

/* Function to draw two axis. */ 

/* */ 

/* •*•** NOTE ■*** */ 

/* The upper left corner of the screen is 0,0 and the */ 

/* bottom right corner is 199,639. */ 

/* */ 



/**********************************************************/ 

int axis(ulrow,ulcol ,lrrow,lrcol ,mantx ,manty) 
int ulrow,ulcol , lrrow, lrcol ; 
double mantx C 3 , mantyC 3 ; 

i nt count , i row , i , iaux , i col , t count ,cinc,rinc,hinc; 
doubl e 

del tax , del tay , ymax , xmin , x , y , rowmax ,rowmi n ,col max , col min; 
double del tar ,deltac ,aux ; 

ymax=manty C03 ; 
xmin=mantx C 1 3 ; 

del tay=0. 1* (manty CO] -mantyC 13); 
del tax=0. 1* < mantx CO 3 -mantx C 1 3 ) ; 

rowmax=l rrow; rowmin=ulrow; del tar=0. 1* (rowmax-rowmin) ; 
colmax=lrcol ; colmin=ul col ; del tac=0. 1* (colmax-colmin) ; 
rinc=del tar+0. 5; cinc=del tac+0. 5; 

/* */ 

/* do vertical line with scale */ 

i =ulcol ; 

for (irow ^ ulrow+1; irow <= lrrow; irow++) bi oswd (irow , i ) ; 
for (irow .= ulrow; irow <= lrrow; irow+=rinc) 

for (tcount=i-4; tcount <= i; tcount++) 

bi oswd ( i row , tcount ) ; 

/* do horizontal line with scale */ 



i=lrrow; 

for (icol = ulcol+1; icol <= lrcol; icol++) bi oswd ( i , i col ) ; 
for (icol = ulcol; icol <= lrcol; icol+=cinc) 

for (tcount=i; tcount <= i+4; tcount-*"*-) 

bioswd (tcount , icol ) ; 

/* */ 

/* write vertical scale */ 
y=ymax ; 

for (irow = ulrow; irow <= lrrow; irow+=rinc) 

C 



i =i row/8. 0+0. 5; 
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bi osset (i , 0) ; 
print-f ("■/.. 1-f " ,y) 
y-=del tay; 

> 

/* write horizontal scale */ 
x=xmin ; 

■for (icol - ulcol; icol <= Ircol; icol+=cinc) 
£ 

i =i col *0. 125-1 . 5; 
bi osset (24, i ) ; 
print-F ("*/.. If" ,x) ; 
x+=del tax ; 

> 



/* — */ 

/* done */ 
return (0) ; 

> 

plot (yymax , xxmax , xxmi n , yymin ,x,t, choice) 
f 1 oat yymax , xxmax ,xCl,tCl,xxmin, yymin; 
int choice; 
f 

f 1 oat x sc al e, y seal e, height , wi dth , bx , by ; 
float ymax , xmax , ymi n , xmin ; 

double xaux , yaux , mantx C21 , manty C2H , 1 og 10 ( ) ; 
double aux , pow ( ) , aux 1 ; 

int i , i nc , row , col , ixexp , iyexp , c , ol drow , newrow , i i , i imax ; 
int coll, col 0 , rowl , rowO ,irow,oldcol ,icol ,newcol ; 
float del tac , del tar , del ta , ang ; 

/* DETERMINE THE SCALING */ 

ymax=yymax; xmax=xxmax; ymin=yymin; xmin=xxmin; 
if (ymax==ymin) 

ymax=ymax+5. 0; 
ymin=ymin-5. 0; 

} 

if (xmax==xmin) 

x max =x max +5 . 0 ; 
xmin=xmin-5. 0; 

> 

hei ght=ymax-ymi n ; 
wi dth=xmax-xmi n; 
xscal e=560. 0/wi dth ; 
yscal e=-160. 0/hei ght ; 
by=184. 0-ymi n*yscal e+0. 5; 
bx=600. 0-xmax*xscal e+0. 5; 
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/* DETERMINE EXPONENT and MANTISSA */ 
xaux=l . 0; 

aux s abs(xmax) ; aux l=abs (xmin ) ; 
i-f (aux < auxl) aux=auxl; 
i -f (aux ! =0. 0) 

C 

xaux=l oglO (aux ) ; 
i xexp=xaux ; 

i-f (ixexpCO) ixexp-=l; 
aux=ixexp ; 
xaux*pow( 10. 0,aux> $ 

> 

mantx CO]=xmax/xaux ; 
mantx C 1 ]=xmin/xaux ; 

yaux=l . 0; 

aux=abs(ymax) ; aux 1-abs (ymin ) ; 
i-f (aux < auxl) aux=auxl; 
i-f (aux ! =0. 0) 

t 

yaux=loglO (aux ) ; 
iyexp=yaux ; 

i-f (iyexp<0) iyexp-=l; 
aux=i yexp ; 
yaux=pow (10. 0,aux ) ; 

> 

mantyC03=ymax/yaux ; 
manty C 1 ]=ymin/yaux ; 

print-f ( "\nXs" , "Press any key to see plot."); 
c=keyrd ( ) ; 
biosi ni (6) ; 

axis (24,40, 1B4 , 600, mantx , manty) ; 

yaux=yaux ; 
biosset (3,6) ; 
print-f ("■/.. le" ,yaux ) ; 

x aux -x aux ; 
biosset (22,68) ; 
printf ("7.. le" ,xaux) ; 
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/**********************************************************/ 
/* */ 

/* PLOT USING LINEAR INTERPOLATION BETWEEN POINTS */ 
/* */ 

/#+■#■*****■*■*■**■*■*■*■*■****■*■*■**■****■**■*■******■***■*■***■**■*****■*■*■*+*■**/ 

if <choice==l) 

£ 

col 0=tC03*x seal e+bx ; 
rowO=x C01*yscal e+by; 
b i oswd ( rowO , col 0 ) ; 

for (i = l; i<npt; i++) 

£ 

col l=t Ci l*xscal e+bx ; 
rowl=x C i l*yscal e+by; 
del tac=col 1-col 0; 
del tar=rowl-rowO; 

if (abs (del tac ) >=abs (del tar ) ) 

£ 

ang=del tar/del tac; 
i imax=abs (col 1-colO) ; 
i col =1 ; 
col =col 0; 
if (colKcolO) 

£ 

i col =-l ; 

for ( i i =0; i i < i imax ; i i ++) 

£ 

col +=i col ; 

del ta=col -col 0; 

row=ang*del ta+row0+0. 5; 

/* if (choice==l) 

£ 

if (abs (del tar) > 5 ?<& abs (del tac) >5) 
goto discont; 

> */ 

bioswd (row, col ) ; 

} 

> /* end_if abs (del tac ) >=abs (del tar ) */ 

else if (abs (del tac) <abs (del tar ) ) 

£ 

ang=del tac /del tar ; 
i i max=abs (rowl-rowO) ; 
i row=l ; 
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row=rowO; 

i-f (rowKrowO) 

£ 

i row=-l ; 

> 

for ( i i=0; i i < i i max ; i i ++) 

£ 

row+=irow; 

del ta=row-rowO; 

col=ang*delta+colO+0. 5; 

/* if (choice==l) 

£ 

if (abs(deltar) > 5 && abs(deltac) >5) 
goto discont; 

> */ 

bioswd (row, col ) ; 

} 

> /* end_else_if */ 

colO=coll; rowO=rowl; 

> /* end_if for i =0,200 */ 

J /* end_i f choice=l *•/ 

^ *************/ 



/* */ 

/* This portion is to be used if you want just the */ 

/* points without linear interpolation. */ 

/* */ 



/**********************************************************/ 

if (choice==2) 

£ 

for <i=0; i<npt; i++) 

£ 

col=t Ci ]*xscal e+bx ; 
row=x Ci D*yscal e+by; 
bioswd (row, col ) ; 

> 

> 

goto termino; 
discont: 

£ 

biosini (3) ; 

pri ntf ( "\n DISCONTINUOUS DATA FOUND."); 
printf ( " CHANGE OPTION to SIMPLE"); 
pri ntf ("-DOT PLOT and TRY AGAIN."); 
printf (" Press any key to continue ."); 
c=keyrd ( ) ; 

> 

termino: 

> 
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APPENDIX H 



LISTING OF THE BATCH FILES - IBM-PC 
/****************************************■* *■*■*•*■*•-*■*■*-* *■*■*■*****/ 



/* */ 

/* Batch -file "CE. BAT" (IBM-PC). Contains the */ 

/* sequence of calls to compile thefile KR.C . ■*/ 

/* */ 

/* USAGE: "ce kr" */ 

/* */ 



/*********************************HHf***********-*Ht- *•■*•*•* •**■*■*■**■/ 

lcl */. 1 
1 c2 7.1 

link cs+%l+biosio+tr+dostime,%l ,nul , lcms+lcs 

erase Xl.obj 

pause 
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/*********#*■*■**■*■**■*■*■*■*■*■*##■**■*■*■*•*■■*■*■***■*■*#*##*■*##*#***#*#****/ 



/* */ 

/* Batch file "KAL.BAT" (IBM-PC). Contains the */ 

/* sequence of calls to execute the files KR.C */ 

/* (Kalman Filter program) and PLOT.C (the */ 

/* plotting routine). */ 

/* USAGE: " kal " */ 

/* */ 



/******■»*■»•»•»•»**■»****■»■»****■♦*******■»■»■»■»■»■»■**+*■»■»■»**•»■»•»■»■»***■»*/ 

graphics 
: loop 
kr =4000 

rem DO YOU WANT TO PLOT RESULTS ? 
pause 

plot =8000 

rem DO YOU WANT TO TRY ANOTHER CASE ? 

rem CTL-BREAK will stop execution. Press any other key to 

continue. 

pause 

CLS 

goto loop 
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APPENDIX I 



SIMTERM - PROGRAM DOCUMENTATION 

This Appendix contains the listing o-f the -file "readme" 
found in the PC-SIG LIBRARY (Public Domain Library) , disk 
number 362. 
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1. Introduction 

Feel -free to make copies o-f this program. There is 
no restriction on the distribution as long as the copyright 
notices are left on the source and that there is no charge 
for the distribution. This program is made available to 
users of IBM-PCs (and compatibles since I really like the 
COMPAQ/PLUS). No guarantees are made or implied. If you have 
any questions or suggestions, please give me a call and I 
will try to answer them. A recent acquisition of an 
interrupt driven communication driver from the University of 
British Columbia Computer Center has enabled me to remove 
the requirement that you have the IBM Async Communications 
Support package which included a driver I used to use. This 
version of SIMTERM is therefore sel f -contai ned and requires 
no other software to run. Have fun using it and **HAPPY 
COMPUTING**. 

This floppy contains all the software for the 
terminal simulator for the IBM PC. The main function of 
this program is to provide a reasonable terminal simulation 
on the IBM PC of an HP-like terminal to a UNIX(TM) system. 
The file 'simterm.exe' is the load module and when invoked, 
assumes a default of 1200 baud, 1 stop bit, no parity, and 
flow control (X0N/X0FF) . The only programs that have to be 
on the floppy are SIMTERM.EXE, HELR.TXT, and MENUS. If you 
have a VENTEL or a Hayes modem, the file BOARDS can be 
copied over as a starter for a list of BBSs. The BOARDS file 
is set up for a VENTEL autodialer. The dialing strings will 
have to be changed to conform for the Hayes standard. 

If you have DOS 2.0, put SIMTERM.EXE, HELP. TXT , 
BOARDS and MENUS in a subdirectory called \simterm. That 
way, if you change directories during a SIMTERM session, by 
specifying a path for any type of file transfer, SIMTERM 
will be able to find its way back to these two important 
files. If you don't have DOS 2.0, then just IGNORE the 
occasional "directory not found" messages, they're harmless. 

2. Command Line Parameters 

Optional parameters on the command line will 
override the defaults as follows: 
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'-a' 


put the simulator into the ADM3a emulation mode 
■for those systems which don't support TERMCAP. 


'-b' 


Causes SIMTERM to assume you are using a 
monochrome display, even though you may not really 
have one. 


’ -B ' 


will cause the transmit and receive bu-f-fers to be 
cleared when a BREAK (End key) is sent. This is 
mainly -for high baud rate lines where there is 
still junk in the buffers that you don't want to 
see. 


’-cn' 


specifies which COMx device you want to use. The 
default is 1 (use C0M1). 


' -d ' 


Enter direct connect mode. SIMTERM won't wait for 
carrier detect. Use this option if you're 
connected directly to the remote, with no modem. 
You can also use this option when SIMTERM seems to 
hang at startup, with the "Establish 
communi cat i ons link" message. This may be case 
with Hayes modems if you don't have them optioned 
to raise the carrier detect. 


* -D ' 


When this flag is used, SIMTERM will ignore DEL 
(127 Ascii) characters, and the character 
immediately following the DEL. We use it because 
we have a noisy line. 


'-e' 


specifies that the EPSON printer does not have the 
GRAFTRAX option. Therefore when using 'ibmpr' to 
route stuff to the printer, the code will simulate 
underlining, subscripts, and superscripts. Default 
is an EPSON with GRAFTRAX. 


'-g' 


Causes SIMTERM to assume you are using a graphics 
display, even though you may not really have one. 


' -h * 


SIMTERM will operate in the ' hal f -dupl ex ' mode; 
i.e. , it will echo the characters the user types 
since the host system does not. 


' -in ' 


specifies the IRQ (interrupt request level) to be 
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used. De-fault is 4, but some boards (like the 
Quadram) use IRQ3 -for COM2, so in this case 
SIMTERM would have to be invoked with the 

parameters ‘-c2 -i3' to set it up correctly. 


'-m' 


Tells SIMTERM that you have a Hayes modem instead 
o-f the de-fault VenTel modem. This will enable 
SIMTERM to use the directory dialing -features 
<Alt-V> 


* — M ' 


Rede-fines the cursor control keys compatible with 
a set of UNIX-like utilities on a PRIME computer 
at UofVa. 


* -nxxxx ' 


if . you have a Ventel or Hayes modem, this option 
will automatically dial the number when the 
simulator is started. 


‘-o' 


turns off the error messages caused by 
communications line; e.g., framing error and 
invalid parity. 


*-px' 


specifies the parity where ‘n'-none, ‘o'-odd, 

‘e'-even, ‘m'-mark, and ‘s '-space. 


* -PF-f il ' 


Use file "file" as a script source file (see 
SCRIPTS below) 


‘ -PDscr ' 


Execute script "scr" upon start-up (see SCRIPTS 
bel ow) 


’-r ' 


this causes SIMTERM to wait for the horizontal 
retrace when writing to the screen. If you notice 
‘sparkles' on your graphics monitor, invoke 
SIMTERM with this option and it should clear up. 


‘ -snnnn ' 


specifies *nnnn' as the baud rate (e.g., -s300) . 


‘-Sn ' 


specifies the number of stop bits (1 or 2) 


‘ -vxxxx ' 


specifies the file (xxxx) to be used when the 
Alt-V options is invoked to dial BBS. The full 
path name of the file should be specified. 
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* -wn ' set the word length to ‘n'. De-fault is S (7 data + 

1 parity) . 

’-xn' this option will turn o-f-f the XON/XOFF -flow 

control. The program will send an XOFF when the 
receive bu-f-fer is almost -full and then an XON 
when it has emptied out. It will also recognize an 
XOFF -from the host to stop transmitting and then 
an XON to start again. 

* -k-f i lename ' will read in the -file ‘-filename' to 

initialize the -function keys. The -file is o-f the 
■form 



n=string o-f chars 



where 0< =n< =9 

ex amp 1 e 
0=who A V 
l=who I wc^V 

I-f the string is terminated with a * ' (chr(l74) 

— obtained by holding down the ‘Alt' key and typing 174 on 
the key pad), a ‘carriage return ' will be appended. 



For example: 

I-f you want to run at 9600 baud, even parity, 
without waiting -for carrier detect 
simterm -s9600 -pe -d 

I-f you want to use COM2 (and it requires IRQ3) at 
300 baud and run script "xyzzy" -from -file "scr2" 

simterm -s300 -c2 -i3 -PFscr2 -PDxyzzy 

I-f you are using a Hayes modem and want a number 
dialed when SIMTERM is started, use the -following command 
line: 



simterm -m -d -n 12145551212 
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3. Program Functions (Unix specifics) 



Once loaded, the program will use all available 
memory above the module -for save space -for paging through 
previous output. I have 256K on my system and this provides 
about 20 pages ('''500 lines) of saved text. The TERMCAP entry 
for the terminal is in the file ‘ termcap . ibm ' . This should 
either be added to your termcap library or the 'shell' 
variable TERMCAP should be set to it before trying to use 
any package that requires cursor addressing. The TERMCAP 
entry makes use of the fact that 'vi' (and 'curses') will 
use a special sequence to enter and leave the control mode. 
The simulator uses these sequences to redefine the cursor 
control keys so that they can be used in that mode; e.g., in 
' vi ' the "up arrow" will send in a "k", while the "PgDn" 
will send in a ""'D" to scroll the display down, etc. For 
those of you who are running on UNIX systems that use 
"terminfo", there is a file "termi nf o. i bm" which has the 
appropriate definitions. 

Output to the printer can be initiated under 
program control with a filter similar to ' hp ' which is used 
for HP terminals. The source file for 'ibmpr' (which is the 
filter that use on UNIX) is on the floppy. You should 
compile this UNIX and then use for controlling output to the 
printer. A typical use of this filter is: 

nroff -T37 file ! ibmpr -p 

The ' -p ' option says to route the output to the 
EPSON printer. The *-i' option will cause underlined text 
to come out in italics. The ' -c ' option is used to invoke 
the compressed mode (132 characters/1 ine) . 

If those escape sequences are output, they will be 
displayed appropr i atel y on the CRT and output to the printer 
correct 1 y. 



4. User Defined Function Keys 

There are 10 user definable functions keys that 
will send a character sequence of up to 30 characters. To 
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display the current contents of the keys, hold the 'Alt' key 
down and type ' = '. To change the contents, hold the 'Alt' 
key and type and -follow directions. To invoke the 
-functions keys, hold down the ‘Alt' key and type the 
corresponding digit <e.g., 'Alt 3'). A user can define a 
file with the contents of the keys and cause it to be read 
in with the ‘ -k ' option on the command line. 

5. Upload/Download (Unix Specific) 

This is used to transfer 'text' files (ones that 
only contain printable ASCII and tabs) between the PC and 
the host. To remember which way the data flow, just remember 
that the IBM PC looks 'up' to UNIX, while UNIX looks 'down' 
on the PC. To move files between the PC and UNIX, hit 'Cntl- 
PgUp ' to transfer PC->UNIX. Answer the prompts. Hit 'Ctrl- 
PgDn ' to move from UNIX- >PC. As the transfer is taking 
place, a message will indicate the # of bytes transferred. 
When going UNIX->PC, there will be more bytes on the PC disk 
than on UNIX because of added * end-of -1 ine ' and 'end-of- 
text ' bytes. This can only be used to transfer TEXT files. 

6. Upload/Download (Straight ASCII & XMODEM) 

The program can be used to connect to BBSs and 
any other systems that support async devices. To download a 
file to the PC, go through the normal dialog on the BBS and 
then when it is waiting for you to hit return to start the 
transfer, press Alt-C to open the 'copy' file that will 
receive the data. You will be put back to the original 
screen where you can hit return. At the end of the data 
transfer, hit Alt-C to close the copy file. 

To upload a file from the PC to a BBS, again go 
through the dialog to setup the transfer and when the BBS 
is ready, press Alt-D to specify the data file. This will 
then 'dump' the file to the comm line. At the end of the 
transfer, you may have to type in a control sequence to 
indicate that you are done. This option will request a 
'delay' between characters so that you do not overrun the 
receiving system. Use '0' for no delay; a value of *1' is 
probably sufficient for most systems that can not handle the 
normal baud rate. Experiment. 
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SIMTERM supports the XMODEM protocol for data 
transfer from a BBS. To invoke this option, hit Alt-R to 
receive with XMODEM at the point that the sender is ready. 
You will be prompted for the file to receive into and then 
the PC will initiate the transfer of data. Transmitting with 
XMODEM is invoked with Alt-T and prompts will follow. 

7. Script files 

Script files are used to allow SIMTERM to do 
mindless interactive login and other such sequences. A 
Scriptfile can be created by your favorite word processor 
and SIMTERM will compile it for you, and save the compiled 
version so you don't have to re-compile at each run. Script 
files read much like English, so they are both easy to 
understand and create. 

Script Commands Available 

Each of the fallowing commands can appear in a 
script file. Note that each separate command must be 
separated by a space, tab or newline. The compiler ignores 
extra white-space, so you can have indenting, multiple 
commands per line, or whatever you want. Syntax notes: "str" 
in the following denotes a string constant delimited by 
quotes. A string can have multiple words. To get a quote 
character into a constant, write it as \". To get a 
backspace, write it as \\. Although one-word strings need 
not have quotes, they should, so that they are not confused 
with keywords (commands) . Non-quoted strings generate 
warnings from the compiler. 

name "str" defines an entry point for the 

script that you can use to identify this part, i.e, if a 
"name whuxlb" appears in the file, then you can start 
running the script by using the name "whuxlb". (See "running 
scripts" below) If you choose "whuxlb" from the pop-up menu 
later, the script will start executing at the statement 
immediately following the "name" statement. 

send "str" sends the string to the remote 

computer, with a carriage return tacked onto the end. Some 
escapes can be placed into the string, as follows: an "\1" 
causes a one second pause when it is encountered. "\m" sends 
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a carriage return. "\j" sends a line-feed. "\c" suppresses 
the carriage return that is normally tacked onto the end of 
a string. Remember, though, you need to type TWO backspaces 
to get one, so these sequences should appear as \\l,\\m, etc 
in your file. 

dial "str" dials the string on a VenTel 

dialer. Obviously, if you don't have a ventel , don't use 
this command (Fake it with "send"). For example, to dial 
"3395" on a Hayes, put a line like "send ATDT3395" in your 
file. 



say "str" outputs "str" onto the screen , so 

that you can put tracer statements in the script. 

goto "str" branches to str. labels to be 

branched to must be followed by a colon. I.e. early in the 
script you have a line "label:" all by itself. Later, you 
can call "goto label" (no colon) to branch back up to the 
earlier statement 

gosub "str" like goto, but when a "return" 

statement is encountered later, the program branches BACK to 
the statement that follows the gosub. Gosubs may be nested. 

return returns from a "gosub" statement 

input "str" Prompts the user with "str", 

reads one line, and sends it to the remote computer. 

settime "str" sets the timeout value to "str" 

seconds. Default is fifteen. 

quit execution is returned to SIMTERM. 

There is an understood "quit" at the end of file 

The Logical Operators (IF/CASE) 

Besides the COMMANDS, the script compiler 
recognizes two logical operators, the if/else statement and 
the case. 



IF SYNTAX 
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if "str" stmntl else stmnt OR 



if "str" i 



stmnt 

stmnt 



OR 



if "str" stmntl (no ELSE) 



> else stmnt 



OR > else < 
stmnt 



} 



When encountered, the program will scan the output 



from the remote computer until either it finds "str" or it 
times out. If it finds "str", stmntl is executed, if it 
times out, stmnt2 is executed. Execution then drops down to 
the next statement, unless the stmntl or stmnt2 contained a 
goto or gosub or quit. 



stmntl or stmnt2 may be either single commands 



from above, or multiple commands delimited by -C and > (note 
white space around the brackets. If the else following the 
if is missing, then execution falls to the next statement. 
Note that ifs may be nested. For example 



if "str" dial "4244" else if "str2" dial "4324" 

Be aware, however, that input is only scanned 



once, so if "str2" appears before "str" in the preceeding 
example, both tests will fail!! 



The case statement is like multiple simultaneous IFs. 
It allows you to set up a few strings to be searched for 
SIMULTANEOUSLY, with an action specified to be executed if 
and when one of the strings is found. Syntax is 



case 

strl: action 
str2: action 

■ 

otherwise action 
caseend 



CASE SYNTAX 
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Action can be either a single statement (say, 
send, dial, or even an i -f , or another case.) or multiple 
statements surrounded by -C and > (like the if clause.). For 
example, i-f you have 



case 

eric: dial "eric's #" 
holtman: dial "another #" 
caseend 



and the input is "djs-fhj g-fjg eric holtman" then "eric's #" 
will be dialed and the case will quit. Note that "holtman" 
is not matched since a case statement executes as soon as 
any o-f the strings is -found. 

Script Examples 

Call number 3395, wait -for "login" prompt. I-f I 
get it, gosub to a login procedure. I-f I don't then say 
something, then quit. The login subroutine will send my id, 
then prompt for my password, send it, then return. 

name example 
dial 3395 

if "login" gosub "subr" else -C 
say "3395 didn't answer" 
qui t 

> 

quit 

subr: 

send "id3434" 

input "Your password, sire? " 
return 

Note that after the "return" the next statement to 
be executed will be "quit". 

Compiling Scripts 

To use the example above, copy it out to a file 
called, for example, "prog". Then invoke SIMTERM with a - 
PFprog flag, like so: "simterm -PFprog". This tells SIMTERIi 

that you wish to use scripts from the file "prog". If no -PF 
flag appears, SIMTERM will read the file "scripts" which is 
where you may want to keep compiled scripts. After SIMTERM 



Copyright @ 1982,1984 



Jim & Eric Holtman 



222 



SIMTERM Instructions 



Page: 1 1 



gets running, type an Alt-L. This informs SIMTERM that you 
wish to use a script. It will examine the file "prog" and 
determine that it needs to be compiled. Tell SIMTERM what 
file you want to put the compiled stuff in, then sit back 
and watch for error messages. After it compiles, hit return 
and proceed to the next step. Remember the file name you 
used for the compiled scripts, and use it next time you run 
SIMTERM, so that you don't have to recompile everything 
again. 

Using a script 

After you type Alt-L, or after the compile is 
finished, a menu of all "name" strings will pop up. If 
you're still using the example, only one name ("example") 
will be there. Using the up and down arrows, make your 
choice, then hit the space bar. SIMTERM will then execute 
your script starting at that named point. 

To execute a script from startup, use the -PD 
flag. For example, to run script "example" from the file 
"prog.scr", type 



simterm -PFprog.scr -PDexample 

8. Functions Available from the Keyboard 

All these functions can be accessed from the 
keyboard by typing the appropriate character. If you forget 
the commands, Alt-H will bring up a menu for you. Just 
follow the menu selection process until you find the desired 
function. Note that the menu will tell you which key to use 
to activate that function in the future. I.e. , the menu has 
Alt-L next to the script choice to remind you that you can 
use Alt-L instead of the menus to activate the script 
functions. 



FI Will 'toggle' the display so that usually 
non-di spl ayed control characters are displayed in reverse 
video using the upper case letter that they are equivalent 
to. 



F2 Will 'print' the line that the cursor is 

positioned at. 
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F3 Send a BREAK 



FS Freezes the display above the cursor. 

Handy to save some data on the upper portion so you can see 
it as you do other 
commands. 



F9 Turns on the row/column display at the 
bottom of the screen so that you know where the cursor is 
currently positioned. 

F10 Toggles XON/XOFF. A convenient way of 
stoppi ng/restart i ng the output to the CRT when connected to 
a system that honors the XON/XOFF protocol for flow control. 

Alt-Fl Will terminate SIMTERM, but leave the DTR 
line high so OR that you can restart SIMTERM and pickup 
where you left Alt-255 off. 

Alt-F2 Terminate SIMTERM, and drop DTR so that 
the modem will hang up. 

Alt-F3 Toggles the DTR (data terminal ready) 
lead on the communications line. This is used to drop the 
carrier at the modem so that you can redial. 

Alt-F4 Toggles the graphics-mode. This will 
display characters with their parity bits so that the 
‘graphics' characters of the PC 0127) are should. This is 
used on some of the BBSs. 

Alt-F5 Clears line 25 if it becomes cluttered. 

Alt-F6 Display various mode settings on line 25. 

Alt-F7 Resets the communications software. In 
some instances an interrupt might be missed and leave the 
system in a state where it is expecting data that will not 
be sent. If you suspect this, try an Alt-F7 before rebooting 
the software to see if it breaks you out of the situation. 

Alt-C Puts SIMTERM in a 'capture' mode so that 
all input received over the communi cat i on line is logged to 
a disk file. Handy for picking up ASCII text from a 
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system that does not support XMODEM. It will prompt you -for 



a file name. 

Alt-D 


Will 'dump' a ASCII file on the PC to the 



comm line. This is a way of 'uploading' to a system that 



does not support 
prevent overruns. 


XMODEM. You can also specify a delay to 


Alt-E 

exit SIMTERM and 


Toggles the ' vi ' mode. Handy when you 
then come back into it with UNIX still in 



' vi ' . The mode should be TRUE to have the cursor keys work 
•for 'vi'. 

Alt-F Enter the 'local' line editing mode. This 
allows you to make corrections to a line and then reenter 
it. Use the cursor control keys to move to the line, 'Ins' 
and 'Del' to -fixup the line and then hit return to reenter 
the line and leave the line edit mode. 



Alt-H 

function keys. 


Display a 'help' menu listing the defined 


Alt-L 


Invoke the 'script' processor 


Alt-R 


Receive a file using the XMODEM protocol. 


Alt-T 


Send a file using the XMODEM protocol. 


Alt-V 


Will display a menu that is used with the 



'auto-dial' capability o-f the Ventel . Once in this menu, 
select the desired -feature. 



A1 1-= 


Display the user defined function keys 


Alt — 


Change a user defined function key 


Home 


Soto top of file in "vi" (unix specific) 


Ins 


Send the current line to remote (similar 



to the ENTER key on an HP-2621) OR i-f in "vi " toggle insert 
mode. 

Del Clear display -from cursor to bottom OR i-f 

in "vi" delete a character 
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Arrows Move through screen memory in the 

specified direction 

PgUp Back up one page through memory 

PgDn Go down one page in memory 

Cntr+ Toggle output to printer 
PrtSc 



Ctrl + Download (Unix > IBM) 

PgDn 

Ctr 1 + Upload (IBM > Unix) 

PgUp 



9. Remote Control of SIMTERM 

Certain sequences of characters, when received 
over the communications line, will cause SIMTERM to execute 
certain functions, like clearing the screen , moving 
characters, turning the printer on and off. In the following 
descriptions ESC is Escape (ASCII 27),- and Ctrl-X denotes 
the character generated by holding down Ctrl and typing X. 
I.e. Ctrl-A is ASCII 1. Note: in the descriptions "ESC H F" 
for example means ESCAPE, capital H, capital F, with NO 
SPACES. 

ESC A — move the cursor up one line 

ESC B — move the cursor down one line 

ESC C — move the cursor right one space 

ESC D — move the cursor left one space 

ESC E — take SIMTERM out of INSERT mode 

ESC F — remotely program a function key. After 

the F, send a number from 1 to 9 to indicate which key to 
program, or a 0 to program key 10. Everything following the 
numeral, up to, but not including a terminating Ctrl-Z is 
then stored in that function key. (See User Function Keys, 
above) 

ESC H — moves cursor to top left of screen 
(HOME) 

ESC K — clears the rest of the line, from 

cursor position over 
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Copyright 



ESC J 
ESC L 

ESC M 
ESC P 1 

ESC P 2 
ESC P E 
ESC P i 

ESC P P 
ESC P 0 
ESC Q 

ESC R 

ESC T A 

ESC T a 
ESC T R 

ESC T r 

ESC V S 

ESC V s 
ESC Cxc 



— clears display -from cursor to end of 
line, and all lines below the cursor 

— inserts a line above the cursor by 
rolling bottom lines down. Bottom line of 
text is lost. 

— current line is deleted, lower lines 
scrolled up to close the gap 

— turn on printer (Like Ctrl-PrtSc). 
Everything that now appears onscreen 
will be copied to the printer 

— like P 1, but printer is put in 
COMPRESSED PRINT MODE. 

— like P 1, but printer is put in 
EMPHASIZED PRINT MODE. 

— doesn't turn printer on, just sets 
ITALICS mode. So, to print in ITALICS, 
first turn on printer (ESC P 1), then 
have the remote computer send this 

— turn on printer, and set into 
PROPORTIONAL print mode. 

— turn off printer. Screen is no longer 
copied to printer 

— Enter INSERT mode. All subsequent 
characters will be inserted before the 
cursor on this line 

— Deletes the character at current 
cursor position. 

Cause SIMTERM to begin ADM3A 

simulation 

— Cause SIMTERM to end ADM3A simulation 

— Cause SIMTERM to set up to play ROGUE, 
so that arrow keys work. 

— Cause SIMTERM to end rogue playing set 
up 

— Cause SIMTERM to set up for vi , so 

that arrows. Ins, Del, PgUp and PgDn do 
reasonable things in this Unix text 

editor 

— End Vi mode 

— causes character 'c' to be printed x 
number of times, i.e. to print 27 B's, 
send ESC C ESC B, since ESC has an ASCII 
value of 27. 
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ESC &dB 



ESC &dD 

ESC &dH 
ESC &dL 
ESC &d@ 
ESC=y x 



ESC A 



— set screen attribute to a value that 
equals BOLD on the printer (Actual 
screen color varies -from monochrome to 
graphics monitor.) 

— set attribute to correspond to 
UNDERLINE 

— set attribute to be SUPERSCRIPT 

— set attribute to be SUBSCRIPT 

— set attribute back to normal 

— move cursor to position y,x: where y 
and x are the ASCII characters which 
equal the desired position plus 32. I.e. 
to move to position 3,3 send ESC = # #, 
since the ASCII value o-f # (35) minus 32 
is 3 

— request ID. SIMTERM will send back the 
string "IBM PC" terminated by a newline. 



10. XMODEM Under UNIX 

When using XMODEM with a UNIX system, the programs 
'xrecv.c' and 'xtrans.c' should be uploaded and compiled on 
the host. These programs are setup -for the System V release 
of UNIX and may need rework if used on other systems (e.g., 
Berkeley). To send a file from the PC->UNIX, type 'xrecv 
filename' on UNIX and when the prompt comes up to start 
transmission, hit Alt-T and follow the instructions. 

To send a file from UNIX->PC, type 'xtrans 
filename' on UNIX and when the prompt comes up to set up the 
receive, hit Alt-R and follow the instructions. 

If you are sending ASCII Text files, invoke the 
* xrecv '/' xtrans ' with the ‘ — t ' argument so that conversion 
is done between the DOS and UNIX conventions. The XMODEM 
protocol using the * -t ' option is the safest way to transfer 
text files between systems since checks are made for 
successful transmission and any errors introduced by noisy 
lines will be filtered out. 

Note: if the * -d ' option is given to either 
program, the following will occur. xrecv leaves a 
running commentary on the connection status in "xr.err", 
and xtrans leaves a corresponding file called "xt.err". They 
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are used -for debugging purposes. 

11. Using SIMTERM with Other Modems 

SIMTERM has been tailored to work with the VENTEL 
or Hayes modem. If you are using it with another modem then 
make sure that the modem is optioned to present the 'modem 
ready ' signal. I-f you want to be able to 'dial' the phone 
number -from the keyboard, you will also have to option it to 
present the 'carrier detect' signal. SIMTERM will not get 
past its initialization code unless it see both the carrier 
detect and the modem ready signals. 

If you are using an acoustic coupler or a 212 
'dial up', then when the message 'establish communications' 
appears, dial up the computer and enable the ' data ' (e.g., 
hit the DATA button on the 212 or put the receiver in the 
coupler) . 

If you are using any modem (or are directly 
connected) , and SIMTERM seems to hang right after the 
"Establish communications link" message, it probably means 
that your modem (or local connection) is not outputting the 
correct RS— 232 signals for SIMTERM. If the equipment's not 
yours, or you don't understand how to fix the problem, or 
you're just too lazy to bother, try invoking SIMTERM with 
the -d flag. I.e. say "simterm -d" instead of just 
"simterm". This will keep SIMTERM from looking at the state 
of your modem, and your problem will disappear. If it still 
doesn't work, you can always give us a call. 

12. Compiling the Simulator 

If you look at the link control file ‘simterm. Ink ' 
you will see the programs that have to be compiled or 
assembled. You must use the MACRO assembler (masm) . The 
file 'struct. mac' has some macros for structured assembly 
code which makes it easier to write. The file ' mactest . asm ' 
has some examples of the use of the structured macros. The 
'relations' between variable or constants are the same as 
the conditional jumps with the 'j' removed, 'long' can be 
appended to the macro to cause a long jump to be generated. 

'com. asm' is the interface to the Async Comm line. 
The file 'comm.inc' has the entry points defined. 
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The -file 'comm.inc' has the entry points defined. 

’ xxgraph . asm ' has functions for cursor control and such from 
PASCAL programs. The file ' graph. inc' has the entry points 
defined. 



' argl ist . ob j ' and ’ argl ist . inc ' provide ’ argv ' and 
' argc ' for the parameters to the PASCAL program. See 
' mainsim. pas ' for examples of their use. Most of the program 
will be self-explanatory (hopefully). Any questions, give me 
a cal 1 . 



If you want to run SIMTERM on a PC with 128K, you 
will have to have access to the compiler and macro 
assembler. Instead of re-compiling and linking the programs 
in simterm.lnk, use "smterm. Ink " . This will build a SIMTERM 
without VenTel features and without Login Scripts, but it 
also knocks about 50K off the ’ .exe' file. 

13. Funny Error Conditions 

Sometimes SIMTERM will appear to lockup and not 
accept any input from the keyboard (you will hear the bell 
indicating that the input buffer is full). This is usually 
caused by an incomplete ESCAPE sequence that has been sent 
to SIMTERM and it is waiting for the rest of the characters, 
which will probably never come from the computer. To 'break 
out' of this condition, depress at the same time the 'Ctrl', 
'Shift', and 'Alt' keys on the left side of the keyboard. 
This will cause SIMTERM to terminate its 'wait state' and 
continue. This sometimes happens after dialing into a BBS or 
other system since you might get some noise of the line. 

Also the transmitter may have missed an interrupt, 
and what you are typing is not sent. If you suspect this, 
hit Alt-F7. 



If SIMTERM gets into a funny condition that you 
can not get it out of, then terminate it with 'Alt-Fl' and 
then reinvoke it. This procedure will leave the PC attached 
to the system and you can pick up where you left off. 

There have been problems on some systems that use 
PROKEY, NEWKEY, or other software that interprets the 
keyboard. If SIMTERM is locking up on you and you have a 
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program like this loaded, reboot the system without it and 
try it. Whatever those programs are doing, they are not very 
friendly to SIMTERM. 

Jim Holtman Eric Holtman 

35 Dogwood Trail Watson Dorm, Room 142 

Randolph, N. J. 07869 Station #2 

(201) -361-3395 Chari ottesvi 1 1 e, VA 22904-0029 

Net Address: . . . ! harpo ! whuxl b ! jph . . . .’ harpo ! whuxl e ! eri c 

If you add any interesting new features to the 
program, please keep me informed. 

I have several other public domain programs that I 
will be willing to trade for ones that I don't have. Drop 
me a note (or a floppy disk) with what you have and we can 
setup a trade. Changes and improvements are continually 
being added to the program and if you are interested in what 
the changes are, just drop me a note (or a floppy) for the 
latest information. 
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ASSEMBLY LANGUAGE ROUTINES - IBM-PC 
This Appendix contains the listings of the -file 
"biosio.asm" and "dostime. asm" , used in the IBM-PC Kalman 
Filter program to generated graphic output and to evaluate 
execution time respecti vel y . The "biosio.asm" is also used 
by the plotting routine, PLOT.C . The files "biosio.asm" and 
"dostime. asm" are part of the Public Domain Library and can 
be found in the PC-SIG library disk number 216. 
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*** biosio.asm *** 



; IBM— PC microso-ft "C" under PC-DOS v2.00 
* 

; MICROSOFT "C" callable 8088 assembly routines that 
; interface directly with the basic I/O system (BIOS). 

m 

> 

; NOTE — The IBM Technical Reference Manual contains a 
; listing of the BIOS and more complete descriptions of each 

; interrupt . 

• 

9 

; Written by L. Cuthbertson, April 1984 

9 

; ********************************************************* 

■ 

PGROUP GROUP PROG 

PROG SEGMENT BYTE PUBLIC 'PROG' 



PUBLIC BIOS IN I , BIOSSET , BIOSPOS 

PUBL I C B I OSUP , B I OSDWN , B I OSRCA , B I OSWCA , B I OSWC 

PUBLIC B I OSWD , B I OSTTY , B I OSCUR 

PUBLIC COMINI ,COMOUT, COMIN, COMSTAT 

PUBLIC INP , OUTP 



ASSUME CS: PGROUP 

*********************************************************** 
*** VIDEO I/O *** 

NOTE - the video I/O interrupt description starts on page 
A-43 of the Tech Ref Manual. 



*********************************************************** 
Initialize screen I/O using the BIOS set mode call 
synopsis biosini (stype) ; 

int stype; screen type 

0 = 40x25 BW (power on default) 

1 = 40x25 Color 

2 = 80x25 BW 

3 = 80x25 Color 
graphics mode 

4 = 320x200 Color 

5 = 320x200 BW 

6 = 640x200 BW 



; internal use only 

; 7 = 80x25 BW card 

m 

BIOS IN I PROC NEAR 

PUSH BP 

MOV BP,SP 

MOV AL, CBP+41 ; SCREEN TYPE IN AL 
MOV AH , 0 ; SET MODE FUNCTION 

INT 10H ; BIOS VIDEO I/O INTERRUPT 

POP BP 
RET 

BIOSINI ENDP 



Set the current cursor position, 
synopsis biosset (irow, icol ) ; 



■a--** no value returned 
int irow; 0 to 24 

i nt icol ; 0 to 79 



BIOSSET PROC NEAR 

PUSH BP 
MOV BP,SP 
MOV DH , CBP+41 
MOV DL , CBP+61 
MOV BH , 0 
MOV AH, 2 
INT 10H ; 

POP BP 
RET 

BIOSSET ENDP 



ROW 

COLUMN 

CURRENT PAGE NUMBER 
CURSOR POSITION SET FUNTION NUMBER 
VIDEO I/O INTERRUPT 



Return the current cursor postion. 

synopsis iret = biosposO; 

int iret; high order bits contain row 
low order bits contain column 



BIOSPOS 


PROC 


NEAR 




PUSH BP 






MOV 


BP , SP 






MOV 


BH , 0 


; CURRENT PAGE NUMBER 


MOV 


AH, 3 


; CURSOR POSITION 


FUNCTION NUMBER 


INT 


10H 


; VIDEO I/O INTERRUPT 




MOV 


AH , DH 


; MOVE INT RETURN 


INTO FUNCTION RETURN 


MOV 


AL , DL 


; DITTO 




POP 


BP 






RET 








BIOSPOS 


ENDP 
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Read the contents of a given screen cell. 

synopsis iret=biosrca ( ) ; 

int iret; high order bits contain attributes 
low order bits contain character 

NOTE - Attributes are defined on page 13-9 o-f the DOS v2. 0 
manual 



BIOSRCA PROC 


NEAR 


PUSH BP 




MOV BP , SP 




MOV BH , 0 


; ACTIVE DISPLAY PAGE 


MOV AH, a 


; READ CHARACTER + ATTRIBUTES FUNCTION 


INT 10H 


; VIDEO I/O INTERRUPT 


POP BP 




RET 




BIOSRCA ENDP 





Write a character to the screen - with attributes. 

synopsi s bi oswca (char , count , att ) ; 

no value returned **•* 
int char; character to output 

int count; number o-f times to output character 
int att; character attribute 



NOTE - Attributes are defined on page 13-9 of the DOS v2.0 
manual 



B I OSWCA PROC NEAR 

PUSH BP 
MOV BP , SP 
MOV AL , CBP+43 
MOV CX , CBP+63 
MOV BL, CBP+83 
MOV BH , 0 
MOV AH, 9 
INT 10H ; 

POP BP 
RET 

BIOSWCA ENDP 



CHARACTER 

NUMBER OF CHARACTERS TO WRITE 
CHARACTER ATTRIBUTE 
ACTIVE DISPLAY PAGE 

WRITE CHARACTER/ w ATTRIBUTES FUNCTION 
VIDEO I/O INTERRUPT 



Write a character to the screen - no attributes, 
synapsis biaswc (char , count) ; 
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*#* no value returned *** 

int char; character to output 

int count; number of times to output character 



BIOSWC 


PROC NEAR 






PUSH BP 








MOV 


BP,SP 








MOV 


AL, CBP+43 


■ 

9 


CHARACTER 




MOV 


CX, CBP+63 


m 

9 


NUMBER OF CHARACTERS 


TO WRITE 


MOV 


BL ,0; 


m 

5 


CHARACTER ATTRIBUTE - 


- NULL 


MOV 


BH,0 


9 


ACTIVE DISPLAY PAGE 




MOV 


AH, 10 


m 

9 


WRITE CHARACTER ONLY 


FUNCTION NUMBER 


INT 


10H ; 


VIDEO I/O INTERRUPT 




POP 


BP 








RET 










BIOSWC 


ENDP 









Write a dot in graphics mode, 
synopsis bioswd (irow, icol ) ; 



no value returned *** 
int irow; 
int i col ; 



BIOSWD 


PROC NEAR 






PUSH BP 






MOV 


BP , SP 






MOV 


DX , CBP+41 ; 


ROW 




MOV 


CX , CBP+63 ; 


COLUMN 


MOV 


AL, 1 


GREEN 


COLOR 


MOV 


AH, 12 ; 


WRITE 


A DOT FUNCTION NUMBER 


INT 


10H ; VIDEO I/O 


INTERRUPT 


POP 


BP 






RET 








BIOSWD 


ENDP 







Return the current video state o-f the screen. 



; synopsis iret = bioscur () ; 

5 

; int iret; low bits are the mode currently set 

; (see biosini for description) 

; high bits are the number of columns on 

screen 

BIQSCUR PROC NEAR 

PUSH BP 
MOV BP,SP 
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MOV AH, 15 ; CURRENT VIDEO STATE FUNCTION 

I NT 1 OH ; BIOS VIDEO I/O INTERRUPT 

POP BP 
RET 

BIOSCUR ENDP 

■ 

> 

; *********************************************************** 
; *** communications port *** 

9 

; NOTE - the communications port I/O is described starting 

; on page A-20 of the Tech Ref Manual. 

• 

9 

: *********************************************************** 



Initialize the communi cati ons port, 
synopsis iret = comini (port ,params) ; 



int iret; 
int port; 
int params; 



return status (see comstat) 

communi cati ons port to initialize (0,1) 

bit pattern for initialization - 



7 6 


5 4 


3 2 10 






-BAUD RATE- 


-PARITY- 


-STOP BIT- 


-WORD 


000 - 


no 


XO - NONE 0 


- 1 10 - 7 


BITS 


001 - 


150 


01 - ODD 1 - 


• 2 11 - 8 


BITS 


010 - 


300 


11 - EVEN 






Oil - 


600 








100 - 


1200 








101 - 


2400 








no - 


4800 








111 - 


9600 









COMINI PROC NEAR 

PUSH BP 
MOV BP,SP 
MOV DX , CBP+43 
MOV AL, CBP+63 
MOV AH , 0 
INT 14H ; 

POP BP 
RET 

COMINI ENDP 



COMM PORT TO INITIALIZE 
PARAMETERS 

INITIALIZATION FUNCTION 
COMM PORT I/O INTERRUPT 



Write a character to the communi cati ons port, 
synopsis iret = comout (port ,c) ; 
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int iret; error return ( >127 i-f error occured) 
— see AH under modem control 
int port; communications port to write (0 or 1) 
int c; character to write. 



C0M0UT PROC NEAR 

PUSH BP 
MOV BP , SP 

MOV DX , CBP+4D ; COMM PORT TO WRITE 

MOV AL , CBP+61 ; CHARACTER TO WRITE 

MOV AH , 1 ; WRITE CHARACTER FUNCTION 

INT 14H ; COMM PORT I/O INTERRUPT 

POP BP 

RET 

COMOUT ENDP 



; Read a character -from the communications port. 

; Waits -for character i-f one is not ready. 

; See \comm\i bmtty . c -for example o-f polling comm port 
; -for character without wait. 

; synopsis c = comin(port); 

; int c; character read -from comm port, 

; > 127 i-f error or no character ready. 

; - see AH under modem control 

; int port; communications port to read <0 or 1) 

COM IN PROC NEAR 
PUSH BP 
MOV BP , SP 

MOV DX , CBP+43 ; COMM PORT TO READ 

MOV AH, 2 ; READ CHARACTER FUNCTION 

INT 14H ; COMM PORT I/O INTERRUPT 

POP BP 

RET 

COM IN ENDP 
5 

; Check the line and modem status 

• 

; synopsis iret = comstat (port ) ; 



i nt 

AH 

bit 

bit 

bit 

bit 

bit 

bit 



iret; ; line and modem status 
high order bits contain line control 
7 = time out 

6 = trans shi-ft register empty 
5 = trans holding register empty 
4 = break detect 
3 = -framing error 
2 = parity error 



status 



238 



; bit 1 = overrun error 

; bit 0 = data ready 

i 

; AL - low order bits contain modem status 

; bit 7 = received line signal detect 

; bit 6 = ring indicator 

; bit 5 = data set ready 

; bit 4 = clear to send 

; bit 3 = delta receive line signal detect 

; bit 2 = trailing edge ring detector 

; bit 1 = delta data set ready 

; bit 0 = delta clear to send 

5 

; Note - -from page A-21 of Tech Ref Manual 

■ 

; int port; ; communications port to check 

COMSTAT PROC NEAR 



PUSH 


1 BP 






MOV 


BP,SP 






MOV 


DX, IBP+43 


; COMM PORT TO CHECK 


MOV 


AH, 3 




; STATUS FUNCTION 


INT 


14H 


m 


COMM PORT I/O INTERRUPT 


POP 


BP 






RET 








COMSTAT 


ENDP 






; *********************************************************** 


? 

■ 


*** 


i o 


. asm *** 



IBM-PC 8088 assembly for interface with microsoft "C" 
under PC-DOS 

"C" callable subroutines that provide absolute pointer 
addressing. 

Pointers within a IBM-PC microsoft "C" program are 
relative to the start of the programs data segment. These 
subroutines provide a mechanism to address absolute memory 
1 ocations. 

Supplied by microsoft - commented by L. Cuthbertson, April 
1984 

******■»■#*********■#■*■»***********■»*##*■**********■»*■***■*-#■■*-#■■*■#■*■* 
Read an absolute memory location, 
synopsis c = readabs ( 1 oc) ; 
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int c; contents of memory location 

int loc; absolute memory location in hex 



INP PROC NEAR 

PUSH BP 

MOV BP , SP 

MOV DX , CBP+43 ; GET LOCATION ADDRESS 
IN AL,DX ; READ LOCATION 

XOR AH, AH ; CLEAR HIGH BYTE 

POP BP 
RET 

INP ENDP 



Write an absolute memory location. 



synopsis wr i teabs < 1 oc , c ) ; 



int loc; absolute memory location in hex 
int c; integer to write to memory location 



OUTP PROC NEAR 
PUSH BP 
MOV BP , SP 

MOV DX , CBP+4I ; GET LOCATION ADDRESS 

MOV AX , CBP+63 ; GET ADDRESS OF INTEGER TO WRITE 

OUT DX , AL ; WRITE TO MEMORY LOCATION 

POP BP 

RET 



OUTP ENDP 



PROG ENDS 
END 



*** dost i me. asm *** 

IBM-PC microsoft "C" under PC-DOS 

Microsoft "C" callable 8088 assembly routine that uses a 
DOS function call to obtain and return the hours, minutes, 
seconds, and hundreths of a second as integers. 

synopsis dostime (&hour ,&min ,&sec,&ths) ; 



*-** 


no function value returned *■** 


int 


hour ; 


0-23 military time 


i nt 


min; 


0-59 


int 


sec ; 


0-59 


i nt 


ths; 


0-99 
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; Written by L. Cuthbertson, May 1984 

9 

; *********************************************************** 

PGROUP GROUP PROG 

PROG SEGMENT BYTE PUBLIC 'PROG' 

PUBLIC DOSTIME 
ASSUME CS: PGROUP 

m 

9 

; *********************************************************** 
DOSTIME PROC NEAR 



PUSH BP 




MOV 


BP , SP 




MOV 


AH , 2CH 


; DOS GET TIME FUNCTION NUMBER 


I NT 


21H ; DOS FUNCTION CALL INTERRUPT 


MOV 


AL,CH 


; MOVE HOURS INTO AX 


XOR 


AH, AH 


; CLEAR HIGH BYTE 


MOV 


DI , CBP+4I 


; ADDRESS OF HOURS VARIABLE 


MOV 


CDII , AX 


; MOVE HOURS INTO HOURS VARIABLE 


MOV 


BL,CL 


; MOVE MINUTES INTO BX 


XOR 


BH,BH 


; CLEAR HIGH BYTE 


MOV 


DI , CBP+6I 


; ADDRESS OF MINUTES VARIABLE 


MOV 


CDI 3 ,BX 


; MOVE MINUTES INTO MINUTES VARIABLE 


MOV 


CL, DH 


; MOVE SECONDS INTO CX 


XOR 


CH , CH 


; CLEAR HIGH BYTE 


MOV 


DI , CBP+8I 


; ADDRESS OF SECONDS VARIABLE 


MOV 


CDII ,CX 


; MOVE SECONDS INTO SECONDS VARIABLE 


XOR 


DH , DH 


; CLEAR HIGH BYTE 


MOV 


DI , CBP+IOI 


; ADDRESS OF THS VARIABLE 


MOV 


CDII ,DX 


; MOVE 1/lOOTHS INTO THS VARIABLE 


POP 


BP 




RET 






DOSTIME 


ENDP 




PROG 


ENDS 





END 
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